openpilot v0.5.2 release

old-commit-hash: 0129a8a4ff8da5314e8e4d4d3336e89667ff6d54
2018-08-19 20:36:37 -07:00 · 2018-08-19 20:36:37 -07:00 · b73d457d20
parent b0c5c1001d
commit b73d457d20
47 changed files with 2272 additions and 232 deletions
--- a/.gitignore
+++ b/.gitignore
@ -20,8 +20,6 @@ a.out
 *.class
 *.pyxbldc
 *.vcd
-lane.cpp
-loc*.cpp
 config.json
 clcache

--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@ -8,11 +8,11 @@ Most open source development activity is coordinated through our [slack](https:/

 * Join our slack [slack.comma.ai](https://slack.comma.ai)
 * Make sure you have a [GitHub account](https://github.com/signup/free)
- * Fork the repository on GitHub
+ * Fork [our repositories](https://github.com/commaai) on GitHub

 ## Car Ports (openpilot)

-We've released a guide for porting to Toyota cars [here](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6)
+We've released a [Model Port guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for porting to Toyota/Lexus models.

-If you port openpilot to a substantially new car, you might be eligible for a bounty. See our bounties at [comma.ai/bounties.html](https://comma.ai/bounties.html)
+If you port openpilot to a substantially new car brand, see this more generic [Brand Port guide](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84). You might also be eligible for a bounty. See our bounties at [comma.ai/bounties.html](https://comma.ai/bounties.html)

--- a/README.md
+++ b/README.md
@ -53,12 +53,13 @@ Supported Cars
 | Honda         | Civic 2017                | Honda Sensing        | Yes     | Yes            | 0mph             | 12mph          |
 | Honda         | Civic 2017 *(Hatch)*      | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          |
 | Honda         | Civic 2018                | Honda Sensing        | Yes     | Yes            | 0mph             | 12mph          |
-| Honda         | CR-V 2015                 | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
-| Honda         | CR-V 2016                 | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
+| Honda         | Civic 2018 *(Hatch)*      | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          |
+| Honda         | CR-V 2015                 | Touring              | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
+| Honda         | CR-V 2016                 | Touring              | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
 | Honda         | CR-V 2017                 | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          |
 | Honda         | CR-V 2018                 | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          |
-| Honda         | Odyssey 2017              | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
-| Honda         | Odyssey 2018              | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
+| Honda         | Odyssey 2017              | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 0mph           |
+| Honda         | Odyssey 2018              | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 0mph           |
 | Honda         | Pilot 2017                | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
 | Honda         | Pilot 2018                | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
 | Honda         | Ridgeline 2017            | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          |
@ -96,7 +97,7 @@ Community Maintained Cars

 [[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266).

-Community Maintained Cars are not confirmed by comma.ai to meet our safety model. Be extra cautious using them.
+Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.

 In Progress Cars
 ------
--- a/RELEASES.md
+++ b/RELEASES.md
@ -1,3 +1,11 @@
+Version 0.5.2 (2018-08-16)
+========================
+ * New calibration: more accurate, a lot faster, open source!
+ * Enable orbd
+ * Add little endian support to CAN packer
+ * Fix fingerprint for Honda Accord 1.5T
+ * Improve driver monitoring model
+
 Version 0.5.1 (2018-08-01)
 ========================
 * Fix radar error on Civic sedan 2018
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@ -309,6 +309,7 @@ struct CarParams {
    cadillac @7;
    hyundai @8;
    chrysler @9;
+    tesla @10;
  }

  # things about the car in the manual
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@ -331,13 +331,16 @@ struct Live20Data {
 }

 struct LiveCalibrationData {
+  # deprecated
  warpMatrix @0 :List(Float32);
+  # camera_frame_from_model_frame
  warpMatrix2 @5 :List(Float32);
  calStatus @1 :Int8;
  calCycle @2 :Int32;
  calPerc @3 :Int8;

-  # Maps car space to normalized image space.
+  # view_frame_from_road_frame
+  # ui's is inversed needs new
  extrinsicMatrix @4 :List(Float32);
 }

--- a/common/dbc.py
+++ b/common/dbc.py
@ -4,7 +4,7 @@ import struct
 import bitstring
 import sys
 import numbers
-from collections import namedtuple
+from collections import namedtuple, defaultdict

 def int_or_float(s):
  # return number, trying to maintain int format
@ -28,6 +28,7 @@ class dbc(object):
    bo_regexp = re.compile(r"^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
    sg_regexp = re.compile(r"^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
    sgm_regexp = re.compile(r"^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+    val_regexp = re.compile(r"VAL\_ (\w+) (\w+) (\s*[-+]?[0-9]+\s+\".+?\"[^;]*)")

    # A dictionary which maps message ids to tuples ((name, size), signals).
    #   name is the ASCII name of the message.
@ -36,6 +37,9 @@ class dbc(object):
    # signals is a list of DBCSignal in order of increasing start_bit.
    self.msgs = {}

+    # A dictionary which maps message ids to a list of tuples (signal name, definition value pairs)
+    self.def_vals = defaultdict(list)
+
    # lookup to bit reverse each byte
    self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in xrange(64)]

@ -81,6 +85,30 @@ class dbc(object):
          DBCSignal(sgname, start_bit, signal_size, is_little_endian,
                    is_signed, factor, offset, tmin, tmax, units))

+      if l.startswith("VAL_ "):
+        # new signal value/definition
+        dat = val_regexp.match(l)
+
+        if dat is None:
+          print "bad VAL", l
+        ids = int(dat.group(1), 0) # could be hex
+        sgname = dat.group(2)
+        defvals = dat.group(3)
+
+        defvals = defvals.replace("?","\?") #escape sequence in C++
+        defvals = defvals.split('"')[:-1]
+
+        defs = defvals[1::2]
+        #cleanup, convert to UPPER_CASE_WITH_UNDERSCORES
+        for i,d in enumerate(defs):
+          d = defs[i].strip().upper()
+          defs[i] = d.replace(" ","_")
+
+        defvals[1::2] = defs
+        defvals = '"'+"".join(str(i) for i in defvals)+'"'
+
+        self.def_vals[ids].append((sgname, defvals))
+
    for msg in self.msgs.viewvalues():
      msg[1].sort(key=lambda x: x.start_bit)

--- a/common/ffi_wrapper.py
+++ b/common/ffi_wrapper.py
@ -0,0 +1,42 @@
+import os
+import sys
+import fcntl
+import hashlib
+from cffi import FFI
+
+TMPDIR = "/tmp/ccache"
+
+def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR):
+  cache = name + "_" + hashlib.sha1(c_code).hexdigest()
+  try:
+    os.mkdir(tmpdir)
+  except OSError:
+    pass
+
+  fd = os.open(tmpdir, 0)
+  fcntl.flock(fd, fcntl.LOCK_EX)
+  try:
+    sys.path.append(tmpdir)
+    try:
+      mod = __import__(cache)
+    except Exception:
+      print "cache miss", cache
+      compile_code(cache, c_code, c_header, tmpdir)
+      mod = __import__(cache)
+  finally:
+    os.close(fd)
+
+  return mod.ffi, mod.lib
+
+def compile_code(name, c_code, c_header, directory):
+  ffibuilder = FFI()
+  ffibuilder.set_source(name, c_code, source_extension='.cpp')
+  ffibuilder.cdef(c_header)
+  os.environ['OPT'] = "-fwrapv -O2 -DNDEBUG -std=c++11"
+  os.environ['CFLAGS'] = ""
+  ffibuilder.compile(verbose=True, debug=False, tmpdir=directory)
+
+def wrap_compiled(name, directory):
+  sys.path.append(directory)
+  mod = __import__(name)
+  return mod.ffi, mod.lib
--- a/common/params.py
+++ b/common/params.py
@ -66,9 +66,6 @@ keys = {
 # written: visiond
 # read:    visiond, controlsd
  "CalibrationParams": TxType.PERSISTENT,
-# written: visiond
-# read:    visiond, ui
-  "CloudCalibration": TxType.PERSISTENT,
 # written: controlsd
 # read:    radard
  "CarParams": TxType.CLEAR_ON_CAR_START,
--- a/common/transformations/camera.py
+++ b/common/transformations/camera.py
@ -0,0 +1,115 @@
+import numpy as np
+import common.transformations.orientation as orient
+
+FULL_FRAME_SIZE = (1164, 874)
+W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
+eon_focal_length = FOCAL = 910.0
+
+# aka 'K' aka camera_frame_from_view_frame
+eon_intrinsics = np.array([
+  [FOCAL,   0.,   W/2.],
+  [  0.,  FOCAL,  H/2.],
+  [  0.,    0.,     1.]])
+
+# aka 'K_inv' aka view_frame_from_camera_frame
+eon_intrinsics_inv = np.linalg.inv(eon_intrinsics)
+
+
+# device/mesh : x->forward, y-> right, z->down
+# view : x->right, y->down, z->forward
+device_frame_from_view_frame = np.array([
+  [ 0.,  0.,  1.],
+  [ 1.,  0.,  0.],
+  [ 0.,  1.,  0.]
+])
+view_frame_from_device_frame = device_frame_from_view_frame.T
+
+
+def get_calib_from_vp(vp):
+  vp_norm = normalize(vp)
+  yaw_calib = np.arctan(vp_norm[0])
+  pitch_calib = np.arctan(vp_norm[1]*np.cos(yaw_calib))
+  # TODO should be, this but written
+  # to be compatible with meshcalib and
+  # get_view_frame_from_road_fram
+  #pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
+  roll_calib = 0
+  return roll_calib, pitch_calib, yaw_calib
+
+# aka 'extrinsic_matrix'
+# road : x->forward, y -> left, z->up
+def get_view_frame_from_road_frame(roll, pitch, yaw, height):
+  # TODO
+  # calibration pitch is currently defined
+  # opposite to pitch in device frame
+  pitch = -pitch
+  device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
+  view_from_road = view_frame_from_device_frame.dot(device_from_road)
+  return np.hstack((view_from_road, [[0], [height], [0]]))
+
+
+def vp_from_ke(m):
+  """
+  Computes the vanishing point from the product of the intrinsic and extrinsic
+  matrices C = KE.
+
+  The vanishing point is defined as lim x->infinity C (x, 0, 0, 1).T
+  """
+  return (m[0, 0]/m[2,0], m[1,0]/m[2,0])
+
+def roll_from_ke(m):
+  # note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
+  return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
+                    -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
+
+def normalize(img_pts):
+  # normalizes image coordinates
+  # accepts single pt or array of pts
+  img_pts = np.array(img_pts)
+  input_shape = img_pts.shape
+  img_pts = np.atleast_2d(img_pts)
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1))))
+  img_pts_normalized = eon_intrinsics_inv.dot(img_pts.T).T
+  img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
+  return img_pts_normalized[:,:2].reshape(input_shape)
+
+def denormalize(img_pts):
+  # denormalizes image coordinates
+  # accepts single pt or array of pts
+  img_pts = np.array(img_pts)
+  input_shape = img_pts.shape
+  img_pts = np.atleast_2d(img_pts)
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1))))
+  img_pts_denormalized = eon_intrinsics.dot(img_pts.T).T
+  img_pts_denormalized[img_pts_denormalized[:,0] > W] = np.nan
+  img_pts_denormalized[img_pts_denormalized[:,0] < 0] = np.nan
+  img_pts_denormalized[img_pts_denormalized[:,1] > H] = np.nan
+  img_pts_denormalized[img_pts_denormalized[:,1] < 0] = np.nan
+  return img_pts_denormalized[:,:2].reshape(input_shape)
+
+def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
+  # device from ecef frame
+  # device frame is x -> forward, y-> right, z -> down
+  # accepts single pt or array of pts
+  input_shape = pt_ecef.shape
+  pt_ecef = np.atleast_2d(pt_ecef)
+  ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef)
+  device_from_ecef_rot = ecef_from_device_rot.T
+  pt_ecef_rel = pt_ecef - pos_ecef
+  pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
+  return pt_device.reshape(input_shape)
+
+def img_from_device(pt_device):
+  # img coordinates from pts in device frame
+  # first transforms to view frame, then to img coords
+  # accepts single pt or array of pts
+  input_shape = pt_device.shape
+  pt_device = np.atleast_2d(pt_device)
+  pt_view = np.einsum('jk,ik->ij', view_frame_from_device_frame, pt_device)
+
+  # This function should never return negative depths
+  pt_view[pt_view[:,2] < 0] = np.nan
+
+  pt_img = pt_view/pt_view[:,2:3]
+  return pt_img.reshape(input_shape)[:,:2]
+
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@ -0,0 +1,112 @@
+import numpy as np
+
+from common.transformations.camera import eon_focal_length, \
+	vp_from_ke, \
+	get_view_frame_from_road_frame, \
+	FULL_FRAME_SIZE
+
+# segnet
+
+SEGNET_SIZE = (512, 384)
+
+segnet_frame_from_camera_frame = np.array([
+  [float(SEGNET_SIZE[0])/FULL_FRAME_SIZE[0],    0.,          ],
+  [     0.,          float(SEGNET_SIZE[1])/FULL_FRAME_SIZE[1]]])
+
+
+# model
+
+MODEL_INPUT_SIZE = (320, 160)
+MODEL_YUV_SIZE = (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1] * 3 // 2)
+MODEL_CX = MODEL_INPUT_SIZE[0]/2.
+MODEL_CY = 21.
+
+model_zoom = 1.25
+model_height = 1.22
+
+# canonical model transform
+model_intrinsics = np.array(
+  [[ eon_focal_length / model_zoom,    0. ,  MODEL_CX],
+   [   0. ,  eon_focal_length / model_zoom,  MODEL_CY],
+   [   0. ,                            0. ,   1.]])
+
+
+# BIG model
+
+BIGMODEL_INPUT_SIZE = (864, 288)
+BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2)
+
+bigmodel_zoom = 1.
+bigmodel_intrinsics = np.array(
+  [[ eon_focal_length / bigmodel_zoom,    0. , 0.5 * BIGMODEL_INPUT_SIZE[0]],
+   [   0. ,  eon_focal_length / bigmodel_zoom,  0.2 * BIGMODEL_INPUT_SIZE[1]],
+   [   0. ,                            0. ,   1.]])
+
+
+bigmodel_border = np.array([
+    [0,0,1],
+    [BIGMODEL_INPUT_SIZE[0], 0, 1],
+    [BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1], 1],
+    [0, BIGMODEL_INPUT_SIZE[1], 1],
+])
+
+
+model_frame_from_road_frame = np.dot(model_intrinsics,
+  get_view_frame_from_road_frame(0, 0, 0, model_height))
+
+bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
+  get_view_frame_from_road_frame(0, 0, 0, model_height))
+
+# 'camera from model camera'
+def get_model_height_transform(camera_frame_from_road_frame, height):
+  camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([
+      [1, 0, 0],
+      [0, 1, 0],
+      [0, 0, 0],
+      [0, 0, 1],
+  ]))
+
+  camera_frame_from_road_high = np.dot(camera_frame_from_road_frame, np.array([
+      [1, 0, 0],
+      [0, 1, 0],
+      [0, 0, height - model_height],
+      [0, 0, 1],
+  ]))
+
+  ground_from_camera_frame = np.linalg.inv(camera_frame_from_road_ground)
+
+  low_camera_from_high_camera = np.dot(camera_frame_from_road_high, ground_from_camera_frame)
+  high_camera_from_low_camera = np.linalg.inv(low_camera_from_high_camera)
+
+  return high_camera_from_low_camera
+
+
+# camera_frame_from_model_frame aka 'warp matrix'
+# was: calibration.h/CalibrationTransform
+def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height):
+  vp = vp_from_ke(camera_frame_from_road_frame)
+
+  model_camera_from_model_frame = np.array([
+    [model_zoom,         0., vp[0] - MODEL_CX * model_zoom],
+    [        0., model_zoom, vp[1] - MODEL_CY * model_zoom],
+    [        0.,         0.,                            1.],
+  ])
+
+  # This function is super slow, so skip it if height is very close to canonical
+  # TODO: speed it up!
+  if abs(height - model_height) > 0.001: #
+    camera_from_model_camera = get_model_height_transform(camera_frame_from_road_frame, height)
+  else:
+    camera_from_model_camera = np.eye(3)
+
+  return np.dot(camera_from_model_camera, model_camera_from_model_frame)
+
+
+def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
+  camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
+  bigmodel_frame_from_ground = bigmodel_frame_from_road_frame[:, (0, 1, 3)]
+
+  ground_from_bigmodel_frame = np.linalg.inv(bigmodel_frame_from_ground)
+  camera_frame_from_bigmodel_frame = np.dot(camera_frame_from_ground, ground_from_bigmodel_frame)
+
+  return camera_frame_from_bigmodel_frame
--- a/common/transformations/orientation.py
+++ b/common/transformations/orientation.py
@ -0,0 +1,293 @@
+import numpy as np
+from numpy import dot, inner, array, linalg
+from common.transformations.coordinates import LocalCoord
+
+
+'''
+Vectorized functions that transform between
+rotation matrices, euler angles and quaternions.
+All support lists, array or array of arrays as inputs.
+Supports both x2y and y_from_x format (y_from_x preferred!).
+'''
+
+def euler2quat(eulers):
+  eulers = array(eulers)
+  if len(eulers.shape) > 1:
+    output_shape = (-1,4)
+  else:
+    output_shape = (4,)
+  eulers = np.atleast_2d(eulers)
+  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
+
+  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
+       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
+  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
+
+  quats = array([q0, q1, q2, q3]).T
+  for i in xrange(len(quats)):
+    if quats[i,0] < 0:
+      quats[i] = -quats[i]
+  return quats.reshape(output_shape)
+
+
+def quat2euler(quats):
+  quats = array(quats)
+  if len(quats.shape) > 1:
+    output_shape = (-1,3)
+  else:
+    output_shape = (3,)
+  quats = np.atleast_2d(quats)
+  q0, q1, q2, q3 = quats[:,0], quats[:,1], quats[:,2], quats[:,3]
+
+  gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
+  theta = np.arcsin(2 * (q0 * q2 - q3 * q1))
+  psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
+
+  eulers = array([gamma, theta, psi]).T
+  return eulers.reshape(output_shape)
+
+
+def quat2rot(quats):
+  quats = array(quats)
+  input_shape = quats.shape
+  quats = np.atleast_2d(quats)
+  Rs = np.zeros((quats.shape[0], 3, 3))
+  q0 = quats[:, 0]
+  q1 = quats[:, 1]
+  q2 = quats[:, 2]
+  q3 = quats[:, 3]
+  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
+  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
+  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
+  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
+  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
+  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
+  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
+  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
+  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
+
+  if len(input_shape) < 2:
+    return Rs[0]
+  else:
+    return Rs
+
+
+def rot2quat(rots):
+  input_shape = rots.shape
+  if len(input_shape) < 3:
+    rots = array([rots])
+  K3 = np.empty((len(rots), 4, 4))
+  K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0
+  K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0
+  K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0
+  K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0
+  K3[:, 1, 0] = K3[:, 0, 1]
+  K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0
+  K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0
+  K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0
+  K3[:, 2, 0] = K3[:, 0, 2]
+  K3[:, 2, 1] = K3[:, 1, 2]
+  K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0
+  K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0
+  K3[:, 3, 0] = K3[:, 0, 3]
+  K3[:, 3, 1] = K3[:, 1, 3]
+  K3[:, 3, 2] = K3[:, 2, 3]
+  K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0
+  q = np.empty((len(rots), 4))
+  for i in xrange(len(rots)):
+    _, eigvecs = linalg.eigh(K3[i].T)
+    eigvecs = eigvecs[:,3:]
+    q[i, 0] = eigvecs[-1]
+    q[i, 1:] = -eigvecs[:-1].flatten()
+    if q[i, 0] < 0:
+      q[i] = -q[i]
+
+  if len(input_shape) < 3:
+    return q[0]
+  else:
+    return q
+
+
+def euler2rot(eulers):
+  return rotations_from_quats(euler2quat(eulers))
+
+
+def rot2euler(rots):
+  return quat2euler(quats_from_rotations(rots))
+
+
+quats_from_rotations = rot2quat
+quat_from_rot = rot2quat
+rotations_from_quats = quat2rot
+rot_from_quat= quat2rot
+rot_from_quat= quat2rot
+euler_from_rot = rot2euler
+euler_from_quat = quat2euler
+rot_from_euler = euler2rot
+quat_from_euler = euler2quat
+
+
+
+
+
+
+'''
+Random helpers below
+'''
+
+
+def quat_product(q, r):
+  t = np.zeros(4)
+  t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3]
+  t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2]
+  t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1]
+  t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0]
+  return t
+
+
+def rot_matrix(roll, pitch, yaw):
+  cr, sr = np.cos(roll), np.sin(roll)
+  cp, sp = np.cos(pitch), np.sin(pitch)
+  cy, sy = np.cos(yaw), np.sin(yaw)
+  rr = array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
+  rp = array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
+  ry = array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
+  return ry.dot(rp.dot(rr))
+
+
+def rot(axis, angle):
+  # Rotates around an arbitrary axis
+  ret_1 = (1 - np.cos(angle)) * array([[axis[0]**2, axis[0] * axis[1], axis[0] * axis[2]], [
+    axis[1] * axis[0], axis[1]**2, axis[1] * axis[2]
+  ], [axis[2] * axis[0], axis[2] * axis[1], axis[2]**2]])
+  ret_2 = np.cos(angle) * np.eye(3)
+  ret_3 = np.sin(angle) * array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
+                              [-axis[1], axis[0], 0]])
+  return ret_1 + ret_2 + ret_3
+
+
+def ecef_euler_from_ned(ned_ecef_init, ned_pose):
+  '''
+  Got it from here:
+  Using Rotations to Build Aerospace Coordinate Systems
+  -Don Koks
+  '''
+  converter = LocalCoord.from_ecef(ned_ecef_init)
+  x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
+  y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
+  z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
+
+  x1 = rot(z0, ned_pose[2]).dot(x0)
+  y1 = rot(z0, ned_pose[2]).dot(y0)
+  z1 = rot(z0, ned_pose[2]).dot(z0)
+
+  x2 = rot(y1, ned_pose[1]).dot(x1)
+  y2 = rot(y1, ned_pose[1]).dot(y1)
+  z2 = rot(y1, ned_pose[1]).dot(z1)
+
+  x3 = rot(x2, ned_pose[0]).dot(x2)
+  y3 = rot(x2, ned_pose[0]).dot(y2)
+  #z3 = rot(x2, ned_pose[0]).dot(z2)
+
+  x0 = array([1, 0, 0])
+  y0 = array([0, 1, 0])
+  z0 = array([0, 0, 1])
+
+  psi = np.arctan2(inner(x3, y0), inner(x3, x0))
+  theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
+  y2 = rot(z0, psi).dot(y0)
+  z2 = rot(y2, theta).dot(z0)
+  phi = np.arctan2(inner(y3, z2), inner(y3, y2))
+
+  ret = array([phi, theta, psi])
+  return ret
+
+
+def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
+  '''
+  Got the math from here:
+  Using Rotations to Build Aerospace Coordinate Systems
+  -Don Koks
+
+  Also accepts array of ecef_poses and array of ned_ecef_inits.
+  Where each row is a pose and an ecef_init.
+  '''
+  ned_ecef_init = array(ned_ecef_init)
+  ecef_poses = array(ecef_poses)
+  output_shape = ecef_poses.shape
+  ned_ecef_init = np.atleast_2d(ned_ecef_init)
+  ecef_poses = np.atleast_2d(ecef_poses)
+
+  ned_poses = np.zeros(ecef_poses.shape)
+  for i, ecef_pose in enumerate(ecef_poses):
+    converter = LocalCoord.from_ecef(ned_ecef_init[i])
+    x0 = array([1, 0, 0])
+    y0 = array([0, 1, 0])
+    z0 = array([0, 0, 1])
+
+    x1 = rot(z0, ecef_pose[2]).dot(x0)
+    y1 = rot(z0, ecef_pose[2]).dot(y0)
+    z1 = rot(z0, ecef_pose[2]).dot(z0)
+
+    x2 = rot(y1, ecef_pose[1]).dot(x1)
+    y2 = rot(y1, ecef_pose[1]).dot(y1)
+    z2 = rot(y1, ecef_pose[1]).dot(z1)
+
+    x3 = rot(x2, ecef_pose[0]).dot(x2)
+    y3 = rot(x2, ecef_pose[0]).dot(y2)
+    #z3 = rot(x2, ecef_pose[0]).dot(z2)
+
+    x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
+    y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
+    z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
+
+    psi = np.arctan2(inner(x3, y0), inner(x3, x0))
+    theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
+    y2 = rot(z0, psi).dot(y0)
+    z2 = rot(y2, theta).dot(z0)
+    phi = np.arctan2(inner(y3, z2), inner(y3, y2))
+    ned_poses[i] = array([phi, theta, psi])
+
+  return ned_poses.reshape(output_shape)
+
+
+def ecef2car(car_ecef, psi, theta, points_ecef, ned_converter):
+  """
+  TODO: add roll rotation
+  Converts an array of points in ecef coordinates into
+  x-forward, y-left, z-up coordinates
+  Parameters
+  ----------
+  psi: yaw, radian
+  theta: pitch, radian
+  Returns
+  -------
+  [x, y, z] coordinates in car frame
+  """
+
+  # input is an array of points in ecef cocrdinates
+  # output is an array of points in car's coordinate (x-front, y-left, z-up)
+
+  # convert points to NED
+  points_ned = []
+  for p in points_ecef:
+    points_ned.append(ned_converter.ecef2ned_matrix.dot(array(p) - car_ecef))
+
+  points_ned = np.vstack(points_ned).T
+
+  # n, e, d -> x, y, z
+  # Calculate relative postions and rotate wrt to heading and pitch of car
+  invert_R = array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]])
+
+  c, s = np.cos(psi), np.sin(psi)
+  yaw_R = array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
+
+  c, s = np.cos(theta), np.sin(theta)
+  pitch_R = array([[c, 0., -s], [0., 1., 0.], [s, 0., c]])
+
+  return dot(pitch_R, dot(yaw_R, dot(invert_R, points_ned)))
--- a/requirements_openpilot.txt
+++ b/requirements_openpilot.txt
@ -1,6 +1,6 @@
-Cython==0.24.1
+Cython==0.27.3
 bitstring==3.1.5
-fastcluster==1.1.21
+fastcluster==1.1.20
 libusb1==1.5.0
 pycapnp==0.6.3
 pyzmq==15.4.0
@ -9,11 +9,11 @@ requests==2.10.0
 setproctitle==1.1.10
 simplejson==3.8.2
 pyyaml==3.12
-cffi==1.7.0
-enum34==1.1.1
+cffi==1.11.5
+enum34==1.1.6
 sympy==1.1.1
-filterpy==1.0.0
+filterpy==1.2.4
 smbus2==0.2.0
-pyflakes==1.5.0
+pyflakes==1.6.0
 -e git+https://github.com/commaai/le_python.git@5eef8f5be5929d33973e1b10e686fa0cdcd6792f#egg=Logentries
-Flask==1.0.1
+Flask==1.0.2
--- a/selfdrive/can/common.h
+++ b/selfdrive/can/common.h
@ -60,10 +60,19 @@ struct Msg {
  const Signal *sigs;
 };

+struct Val {
+  const char* name;
+  uint32_t address;
+  const char* def_val;
+  const Signal *sigs;
+};
+
 struct DBC {
  const char* name;
  size_t num_msgs;
  const Msg *msgs;
+  const Val *vals;
+  size_t num_vals;
 };

 const DBC* dbc_lookup(const std::string& dbc_name);
--- a/selfdrive/can/dbc_template.cc
+++ b/selfdrive/can/dbc_template.cc
@ -48,12 +48,28 @@ const Msg msgs[] = {
 {% endfor %}
 };

+const Val vals[] = {
+{% for address, sig in def_vals %}
+  {% for sg_name, def_val in sig %}
+    {% set address_hex = "0x%X" % address %}
+    {
+      .name = "{{sg_name}}",
+      .address = {{address_hex}},
+      .def_val = {{def_val}},
+      .sigs = sigs_{{address}},
+    },
+  {% endfor %}
+{% endfor %}
+};
+
 }

 const DBC {{dbc.name}} = {
  .name = "{{dbc.name}}",
  .num_msgs = ARRAYSIZE(msgs),
  .msgs = msgs,
+  .vals = vals,
+  .num_vals = ARRAYSIZE(vals),
 };

 dbc_init({{dbc.name}})
--- a/selfdrive/can/libdbc_py.py
+++ b/selfdrive/can/libdbc_py.py
@ -57,10 +57,19 @@ typedef struct {
  const Signal *sigs;
 } Msg;

+typedef struct {
+  const char* name;
+  uint32_t address;
+  const char* def_val;
+  const Signal *sigs;
+} Val;
+
 typedef struct {
  const char* name;
  size_t num_msgs;
  const Msg *msgs;
+  const Val *vals;
+  size_t num_vals;
 } DBC;


--- a/selfdrive/can/packer.cc
+++ b/selfdrive/can/packer.cc
@ -13,9 +13,25 @@

 namespace {

+  // this is the same as read_u64_le, but uses uint64_t as in/out
+  uint64_t ReverseBytes(uint64_t x) {
+    return ((x & 0xff00000000000000ull) >> 56) |
+           ((x & 0x00ff000000000000ull) >> 40) |
+           ((x & 0x0000ff0000000000ull) >> 24) |
+           ((x & 0x000000ff00000000ull) >> 8) |
+           ((x & 0x00000000ff000000ull) << 8) |
+           ((x & 0x0000000000ff0000ull) << 24) |
+           ((x & 0x000000000000ff00ull) << 40) |
+           ((x & 0x00000000000000ffull) << 56);
+  }
+
  uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){
-    uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo;
-    uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo;
+    int shift = sig.is_little_endian? sig.b1 : sig.bo;
+    uint64_t mask = ((1ULL << sig.b2)-1) << shift;
+    uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift;
+    if (sig.is_little_endian) {
+      dat = ReverseBytes(dat);
+    }
    ret &= ~mask;
    ret |= dat;
    return ret;
--- a/selfdrive/can/packer.py
+++ b/selfdrive/can/packer.py
@ -46,9 +46,17 @@ class CANPacker(object):


 if __name__ == "__main__":
-  cp = CANPacker("honda_civic_touring_2016_can_generated")
-  s = cp.pack_bytes(0x30c, [
-    ("PCM_SPEED", 123),
-    ("PCM_GAS", 10),
-  ])
+  ## little endian test
+  cp = CANPacker("hyundai_2015_ccan")
+  s = cp.pack_bytes(0x340, {
+    "CR_Lkas_StrToqReq": -0.06,
+    "CF_Lkas_FcwBasReq": 1,
+    "CF_Lkas_Chksum": 3,
+  })
+  # big endian test
+  #cp = CANPacker("honda_civic_touring_2016_can_generated")
+  #s = cp.pack_bytes(0xe4, {
+  #  "STEER_TORQUE": -2,
+  #})
+  print [hex(ord(v)) for v in s[1]]
  print(s[1].encode("hex"))
--- a/selfdrive/can/parser.py
+++ b/selfdrive/can/parser.py
@ -93,6 +93,44 @@ class CANParser(object):
    libdbc.can_update(self.can, sec, wait)
    return self.update_vl(sec)

+class CANDefine(object):
+  def __init__(self, dbc_name):
+    self.dv = defaultdict(dict)
+    self.dbc_name = dbc_name
+    self.dbc = libdbc.dbc_lookup(dbc_name)
+
+    num_vals = self.dbc[0].num_vals
+
+    self.address_to_msg_name = {}
+    num_msgs = self.dbc[0].num_msgs
+    for i in range(num_msgs):
+      msg = self.dbc[0].msgs[i]
+      name = ffi.string(msg.name)
+      address = msg.address
+      self.address_to_msg_name[address] = name
+
+    for i in range(num_vals):
+      val = self.dbc[0].vals[i]
+
+      sgname = ffi.string(val.name)
+      address = val.address
+      def_val = ffi.string(val.def_val)
+
+      #separate definition/value pairs
+      def_val = def_val.split()
+      values = [int(v) for v in def_val[::2]]
+      defs = def_val[1::2]
+
+      if address not in self.dv:
+        self.dv[address] = {}
+        msgname = self.address_to_msg_name[address]
+        self.dv[msgname] = {}
+
+      # two ways to lookup: address or msg name
+      self.dv[address][sgname] = {v: d for v, d in zip(values, defs)} #build dict
+      self.dv[msgname][sgname] = self.dv[address][sgname]
+
+
 if __name__ == "__main__":
  from common.realtime import sec_since_boot

--- a/selfdrive/can/process_dbc.py
+++ b/selfdrive/can/process_dbc.py
@ -24,6 +24,9 @@ with open(template_fn, "r") as template_f:
 msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
        for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs]

+def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
+def_vals = [(address, sig) for address, sig in sorted(def_vals.iteritems())]
+
 if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
  checksum_type = "honda"
  checksum_size = 4
@ -55,7 +58,7 @@ for name, count in c.items():
  if count > 1:
    sys.exit("Duplicate message name in DBC file %s" % name)

-parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len)
+parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)

 with open(out_fn, "w") as out_f:
  out_f.write(parser_code)
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@ -6,21 +6,6 @@ import numpy as np

 WHEEL_RADIUS = 0.33

-def parse_gear_shifter(can_gear, car_fingerprint):
-  # TODO: Use values from DBC to parse this field
-  if can_gear == 0x0:
-    return "park"
-  elif can_gear == 0x1:
-    return "reverse"
-  elif can_gear == 0x2:
-    return "neutral"
-  elif can_gear == 0x3:
-    return "drive"
-  elif can_gear == 0x4:
-    return "brake"
-  return "unknown"
-
-
 def get_can_parser(CP):

  signals = [
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@ -1,48 +1,17 @@
 from common.numpy_fast import interp
 from common.kalman.simple_kalman import KF1D
-from selfdrive.can.parser import CANParser
+from selfdrive.can.parser import CANParser, CANDefine
 from selfdrive.config import Conversions as CV
-from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD
+from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR

-def parse_gear_shifter(can_gear_shifter, car_fingerprint):
+def parse_gear_shifter(gear, vals):

-  # TODO: Use VAL from DBC to parse this field
-  if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY):
-    if can_gear_shifter == 0x1:
-      return "park"
-    elif can_gear_shifter == 0x2:
-      return "reverse"
-    elif can_gear_shifter == 0x3:
-      return "neutral"
-    elif can_gear_shifter == 0x4:
-      return "drive"
-    elif can_gear_shifter == 0xa:
-      return "sport"
-  elif car_fingerprint in (CAR.CIVIC, CAR.CRV, CAR.ACURA_RDX, CAR.ACCORD_15, CAR.CRV_5G, CAR.CIVIC_HATCH):
-    if can_gear_shifter == 0x1:
-      return "park"
-    elif can_gear_shifter == 0x2:
-      return "reverse"
-    elif can_gear_shifter == 0x4:
-      return "neutral"
-    elif can_gear_shifter == 0x8:
-      return "drive"
-    elif can_gear_shifter == 0x10:
-      return "sport"
-    elif can_gear_shifter == 0x20:
-      return "low"
-
-  elif car_fingerprint in (CAR.ACCORD, CAR.PILOT, CAR.RIDGELINE):
-     if can_gear_shifter == 0x8:
-       return "reverse"
-     elif can_gear_shifter == 0x4:
-       return "park"
-     elif can_gear_shifter == 0x20:
-       return "drive"
-     elif can_gear_shifter == 0x2:
-        return "sport"
-
-  return "unknown"
+  val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
+                  'D': 'drive', 'S': 'sport', 'L': 'low'}
+  try:
+    return val_to_capnp[vals[gear]]
+  except KeyError:
+    return "unknown"


 def calc_cruise_offset(offset, speed):
@ -168,6 +137,8 @@ def get_can_parser(CP):
 class CarState(object):
  def __init__(self, CP):
    self.CP = CP
+    self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
+    self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]

    self.user_gas, self.user_gas_pressed = 0., 0
    self.brake_switch_prev = 0
@ -230,15 +201,17 @@ class CarState(object):
    self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']

    # calc best v_ego estimate, by averaging two opposite corners
-    self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
-    self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
-    self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
-    self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
+    speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
+    self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
+    self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
+    self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
+    self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
    self.v_wheel = (self.v_wheel_fl+self.v_wheel_fr+self.v_wheel_rl+self.v_wheel_rr)/4.

    # blend in transmission speed at low speed, since it has more low speed accuracy
    self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
-    speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS + self.v_weight * self.v_wheel
+    speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
+      self.v_weight * self.v_wheel

    if abs(speed - self.v_ego) > 2.0:  # Prevent large accelerations when car starts at non zero speed
      self.v_ego_x = [[speed], [0.0]]
@ -254,7 +227,6 @@ class CarState(object):
      self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
      self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change

-    can_gear_shifter = cp.vl["GEARBOX"]['GEAR_SHIFTER']
    self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
    self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
    self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
@ -275,7 +247,8 @@ class CarState(object):
      self.brake_hold = 0  # TODO
      self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']

-    self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint)
+    can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
+    self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.shifter_values)

    self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
    # crv doesn't include cruise control
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@ -500,7 +500,7 @@ class CarInterface(object):
      events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
    if ret.gearShifter == 'reverse':
      events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-    if self.CS.brake_hold:
+    if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
      events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
    if self.CS.park_brake:
      events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@ -124,5 +124,19 @@ STEER_THRESHOLD = {
  CAR.RIDGELINE: 1200,
 }

+SPEED_FACTOR = {
+  CAR.ACCORD: 1.,
+  CAR.ACCORD_15: 1.,
+  CAR.ACURA_ILX: 1.,
+  CAR.ACURA_RDX: 1.,
+  CAR.CIVIC: 1.,
+  CAR.CIVIC_HATCH: 1.,
+  CAR.CRV: 1.025,
+  CAR.CRV_5G: 1.025,
+  CAR.ODYSSEY: 1.,
+  CAR.PILOT: 1.,
+  CAR.RIDGELINE: 1.,
+}
+
 # TODO: get these from dbc file
 HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.CIVIC_HATCH, CAR.CRV_5G]
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@ -1,37 +1,17 @@
-from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD
-from selfdrive.can.parser import CANParser
-from selfdrive.config import Conversions as CV
-from common.kalman.simple_kalman import KF1D
 import numpy as np
+from common.kalman.simple_kalman import KF1D
+from selfdrive.can.parser import CANParser, CANDefine
+from selfdrive.config import Conversions as CV
+from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD

+def parse_gear_shifter(gear, vals):

-def parse_gear_shifter(can_gear, car_fingerprint):
-  # TODO: Use values from DBC to parse this field
-  if car_fingerprint in [CAR.PRIUS, CAR.CHRH, CAR.CAMRYH]:
-    if can_gear == 0x0:
-      return "park"
-    elif can_gear == 0x1:
-      return "reverse"
-    elif can_gear == 0x2:
-      return "neutral"
-    elif can_gear == 0x3:
-      return "drive"
-    elif can_gear == 0x4:
-      return "brake"
-  elif car_fingerprint in [CAR.RAV4, CAR.RAV4H, 
-                           CAR.LEXUS_RXH, CAR.COROLLA, CAR.CHR, CAR.CAMRY]:
-    if can_gear == 0x20:
-      return "park"
-    elif can_gear == 0x10:
-      return "reverse"
-    elif can_gear == 0x8:
-      return "neutral"
-    elif can_gear == 0x0:
-      return "drive"
-    elif can_gear == 0x1:
-      return "sport"
-
-  return "unknown"
+  val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
+                  'D': 'drive', 'B': 'brake'}
+  try:
+    return val_to_capnp[vals[gear]]
+  except KeyError:
+    return "unknown"


 def get_can_parser(CP):
@ -89,6 +69,8 @@ class CarState(object):
  def __init__(self, CP):

    self.CP = CP
+    self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
+    self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
    self.left_blinker_on = 0
    self.right_blinker_on = 0

@ -117,7 +99,6 @@ class CarState(object):
                                    cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
    self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']

-    can_gear = cp.vl["GEAR_PACKET"]['GEAR']
    self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
    self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
    self.car_gas = self.pedal_gas
@ -142,7 +123,8 @@ class CarState(object):

    self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
    self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
-    self.gear_shifter = parse_gear_shifter(can_gear, self.car_fingerprint)
+    can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
+    self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
    self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
    self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
    self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@ -1 +1 @@
-#define COMMA_VERSION "0.5.1-release"
+#define COMMA_VERSION "0.5.2-release"
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@ -511,7 +511,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
      prof.checkpoint("State transition")

    # compute actuators
-    actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph, 
+    actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
      v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive)
    prof.checkpoint("State Control")

--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@ -181,11 +181,11 @@ class AlertManager(object):
        AlertStatus.userPrompt, AlertSize.mid,
        Priority.LOW, None, None, 0., 0., .2),

-    "debugAlert": Alert( 
-        "DEBUG ALERT", 
-        "", 
-        AlertStatus.userPrompt, AlertSize.mid, 
-        Priority.LOW, None, None, .1, .1, .1), 
+    "debugAlert": Alert(
+        "DEBUG ALERT",
+        "",
+        AlertStatus.userPrompt, AlertSize.mid,
+        Priority.LOW, None, None, .1, .1, .1),

    # Non-entry only alerts
    "wrongCarModeNoEntry": Alert(
--- a/selfdrive/controls/lib/driver_monitor.py
+++ b/selfdrive/controls/lib/driver_monitor.py
@ -6,7 +6,7 @@ _DT = 0.01                  # update runs at 100Hz
 _AWARENESS_TIME = 180       # 3 minutes limit without user touching steering wheels make the car enter a terminal status
 _AWARENESS_PRE_TIME = 20.   # a first alert is issued 20s before expiration
 _AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 6.
+_DISTRACTED_TIME = 8.
 _DISTRACTED_PRE_TIME = 4.
 _DISTRACTED_PROMPT_TIME = 2.
 # measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model
@ -27,13 +27,6 @@ _VARIANCE_FILTER_F = 0.008    # 0.008Hz, 20s ts
 _VARIANCE_FILTER_K = 2 * np.pi * _VARIANCE_FILTER_F * _DTM / (1 + 2 * np.pi * _VARIANCE_FILTER_F * _DTM)


-class MonitorChangeEvent:
-  NO_CHANGE = 0
-  PARAM_ON = 1
-  PARAM_OFF = 2
-  VARIANCE_HIGH = 3
-  VARIANCE_LOW = 4
-
 class _DriverPose():
  def __init__(self):
    self.yaw = 0.
@ -52,7 +45,6 @@ class DriverStatus():
    self.monitor_on = monitor_on
    self.monitor_param_on = monitor_on
    self.monitor_valid = True   # variance needs to be low
-    self.monitor_change_event = MonitorChangeEvent.NO_CHANGE
    self.awareness = 1.
    self.driver_distracted = False
    self.driver_distraction_level = 0.
@ -89,21 +81,6 @@ class DriverStatus():
    return 1 if metric > _METRIC_THRESHOLD else 0


-  def _monitor_change_check(self, monitor_param_on_prev, monitor_valid_prev, monitor_on_prev):
-    if not monitor_param_on_prev and self.monitor_param_on:
-      self._reset_filters()
-      return MonitorChangeEvent.PARAM_ON
-    elif monitor_param_on_prev and not self.monitor_param_on:
-      self._reset_filters()
-      return MonitorChangeEvent.PARAM_OFF
-    elif monitor_on_prev and not self.monitor_valid:
-      return MonitorChangeEvent.VARIANCE_HIGH
-    elif self.monitor_param_on and not monitor_valid_prev and self.monitor_valid:
-      return MonitorChangeEvent.VARIANCE_LOW
-    else:
-      return MonitorChangeEvent.NO_CHANGE
-
-
  def get_pose(self, driver_monitoring, params):

    self.pose.pitch = driver_monitoring.descriptor[0]
@ -121,17 +98,17 @@ class DriverStatus():

    monitor_param_on_prev = self.monitor_param_on
    monitor_valid_prev = self.monitor_valid
-    monitor_on_prev = self.monitor_on

    # don't check for param too often as it's a kernel call
    ts = sec_since_boot()
    if ts - self.ts_last_check > 1.:
      self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
      self.ts_last_check = ts
-    
+
    self.monitor_valid = _monitor_hysteresys(self.variance_level, monitor_valid_prev)
    self.monitor_on = self.monitor_valid and self.monitor_param_on
-    self.monitor_change_event = self._monitor_change_check(monitor_param_on_prev, monitor_valid_prev, monitor_on_prev)
+    if monitor_param_on_prev != self.monitor_param_on:
+      self._reset_filters()
    self._set_timers()


@ -142,9 +119,6 @@ class DriverStatus():
    if (driver_engaged and self.awareness > 0.) or not ctrl_active:
      # always reset if driver is in control (unless we are in red alert state) or op isn't active
      self.awareness = 1.
-      if not ctrl_active:
-        # hold monitor_level constant when control isn't active
-        self.variance_level = 0. if self.monitor_valid else 1.

    if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \
       not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
@ -163,8 +137,6 @@ class DriverStatus():
    if alert is not None:
      events.append(create_event(alert, [ET.WARNING]))

-    self.monitor_change_event = MonitorChangeEvent.NO_CHANGE
-
    return events


@ -177,5 +149,4 @@ if __name__ == "__main__":
    print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
  ds.update([], True, True, False)
  print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
-  

--- a/selfdrive/controls/lib/vehicle_model.py
+++ b/selfdrive/controls/lib/vehicle_model.py
@ -101,5 +101,5 @@ if __name__ == '__main__':
  VM = VehicleModel(CP)
  #print VM.steady_state_sol(.1, 0.15)
  #print calc_slip_factor(VM)
-  #print VM.yaw_rate(3.*np.pi/180, 32.) * 180./np.pi
+  print VM.yaw_rate(1.*np.pi/180, 32.) * 180./np.pi
  #print VM.curvature_factor(32)
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@ -0,0 +1,246 @@
+#!/usr/bin/env python
+import os
+import zmq
+import cv2
+import copy
+import math
+import json
+import numpy as np
+import selfdrive.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from selfdrive.services import service_list
+from common.params import Params
+from common.ffi_wrapper import ffi_wrap
+import common.transformations.orientation as orient
+from common.transformations.model import model_height, get_camera_frame_from_model_frame
+from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
+                                          eon_intrinsics, get_calib_from_vp, normalize, denormalize, H, W
+
+
+MIN_SPEED_FILTER = 7 # m/s  (~15.5mph)
+MAX_YAW_RATE_FILTER = math.radians(3) # per second
+FRAMES_NEEDED = 120  # allow to update VP every so many frames
+VP_CYCLES_NEEDED = 2
+CALIBRATION_CYCLES_NEEDED = FRAMES_NEEDED * VP_CYCLES_NEEDED
+DT = 0.1      # nominal time step of 10Hz (orbd_live runs slower on pc)
+VP_RATE_LIM = 2. * DT    # 2 px/s
+VP_INIT = np.array([W/2., H/2.])
+EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
+VP_VALIDITY_CORNERS = np.array([[-200., -200.], [200., 200.]])  + VP_INIT
+GRID_WEIGHT_INIT = 2e6
+MAX_LINES = 500    # max lines to avoid over computation
+
+DEBUG = os.getenv("DEBUG") is not None
+
+# Wrap c code for slow grid incrementation
+c_header = "\nvoid increment_grid(double *grid, double *lines, long long n);"
+c_code = "#define H %d\n" % H
+c_code += "#define W %d\n" % W
+c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "get_vp.c")).read()
+ffi, lib = ffi_wrap('get_vp', c_code, c_header)
+
+
+class Calibration:
+  UNCALIBRATED = 0
+  CALIBRATED = 1
+  INVALID = 2
+
+
+def increment_grid_c(grid, lines, n):
+  lib.increment_grid(ffi.cast("double *", grid.ctypes.data),
+                ffi.cast("double *", lines.ctypes.data),
+                ffi.cast("long long", n))
+
+def get_lines(p):
+  A = (p[:,0,1] - p[:,1,1])
+  B = (p[:,1,0] - p[:,0,0])
+  C = (p[:,0,0]*p[:,1,1] - p[:,1,0]*p[:,0,1])
+  return np.column_stack((A, B, -C))
+
+def correct_pts(pts, rot_speeds, dt):
+  pts = np.hstack((pts, np.ones((pts.shape[0],1))))
+  cam_rot = dt * view_frame_from_device_frame.dot(rot_speeds)
+  rot = orient.rot_matrix(*cam_rot.T).T
+  pts_corrected = rot.dot(pts.T).T
+  pts_corrected[:,0] /= pts_corrected[:,2]
+  pts_corrected[:,1] /= pts_corrected[:,2]
+  return pts_corrected[:,:2]
+
+def gaussian_kernel(sizex, sizey, stdx, stdy, dx, dy):
+  y, x = np.mgrid[-sizey:sizey+1, -sizex:sizex+1]
+  g = np.exp(-((x - dx)**2 / (2. * stdx**2) + (y - dy)**2 / (2. * stdy**2)))
+  return g / g.sum()
+
+def blur_image(img, kernel):
+  return cv2.filter2D(img.astype(np.uint16), -1, kernel)
+
+def is_calibration_valid(vp):
+  return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \
+         vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
+
+class Calibrator(object):
+  def __init__(self, param_put=False):
+    self.param_put = param_put
+    self.speed = 0
+    self.vp_cycles = 0
+    self.angle_offset = 0.
+    self.yaw_rate = 0.
+    self.l100_last_updated = 0
+    self.prev_orbs = None
+    self.kernel = gaussian_kernel(11, 11, 2.35, 2.35, 0, 0)
+
+    self.vp = copy.copy(VP_INIT)
+    self.cal_status = Calibration.UNCALIBRATED
+    self.frame_counter = 0
+    self.params = Params()
+    calibration_params = self.params.get("CalibrationParams")
+    if calibration_params:
+      try:
+        calibration_params = json.loads(calibration_params)
+        self.vp = np.array(calibration_params["vanishing_point"])
+        self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
+        self.vp_cycles = VP_CYCLES_NEEDED
+        self.frame_counter = CALIBRATION_CYCLES_NEEDED
+      except Exception:
+        cloudlog.exception("CalibrationParams file found but error encountered")
+
+    self.vp_unfilt = self.vp
+    self.orb_last_updated = 0.
+    self.reset_grid()
+
+  def reset_grid(self):
+    if self.cal_status == Calibration.UNCALIBRATED:
+      # empty grid
+      self.grid = np.zeros((H+1, W+1), dtype=np.float)
+    else:
+      # gaussian grid, centered at vp
+      self.grid = gaussian_kernel(W/2., H/2., 16, 16, self.vp[0] - W/2., self.vp[1] - H/2.) * GRID_WEIGHT_INIT
+
+  def rescale_grid(self):
+    self.grid *= 0.9
+
+  def update(self, uvs, yaw_rate, speed):
+    if len(uvs) < 10 or \
+       abs(yaw_rate) > MAX_YAW_RATE_FILTER or \
+       speed < MIN_SPEED_FILTER:
+      return
+    rot_speeds = np.array([0.,0.,-yaw_rate])
+    uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt))
+    good_tracks = np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10
+    uvs = uvs[good_tracks]
+    if uvs.shape[0] > MAX_LINES:
+      uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :]
+    lines = get_lines(uvs)
+
+    increment_grid_c(self.grid, lines, len(lines))
+    self.frame_counter += 1
+    if (self.frame_counter % FRAMES_NEEDED) == 0:
+      grid = blur_image(self.grid, self.kernel)
+      argmax_vp = np.unravel_index(np.argmax(grid), grid.shape)[::-1]
+      self.rescale_grid()
+      self.vp_unfilt = np.array(argmax_vp)
+      self.vp_cycles += 1
+      if (self.vp_cycles - VP_CYCLES_NEEDED) % 10 == 0:    # update file every 10
+        # skip rate_lim before writing the file to avoid writing a lagged vp
+        if self.cal_status != Calibration.CALIBRATED:
+          self.vp = self.vp_unfilt
+        if self.param_put:
+          cal_params = {"vanishing_point": list(self.vp), "angle_offset2": self.angle_offset}
+          self.params.put("CalibrationParams", json.dumps(cal_params))
+      return self.vp_unfilt
+
+  def parse_orb_features(self, log):
+    matches = np.array(log.orbFeatures.matches)
+    n = len(log.orbFeatures.matches)
+    t = float(log.orbFeatures.timestampLastEof)*1e-9
+    if t == 0 or n == 0:
+      return t, np.zeros((0,2,2))
+    orbs = denormalize(np.column_stack((log.orbFeatures.xs,
+                                        log.orbFeatures.ys)))
+    if self.prev_orbs is not None:
+      valid_matches = (matches > -1) & (matches < len(self.prev_orbs))
+      tracks = np.hstack((orbs[valid_matches], self.prev_orbs[matches[valid_matches]])).reshape((-1,2,2))
+    else:
+      tracks = np.zeros((0,2,2))
+    self.prev_orbs = orbs
+    return t, tracks
+
+  def update_vp(self):
+    # rate limit to not move VP too fast
+    self.vp = np.clip(self.vp_unfilt, self.vp - VP_RATE_LIM, self.vp + VP_RATE_LIM)
+    if self.vp_cycles < VP_CYCLES_NEEDED:
+      self.cal_status = Calibration.UNCALIBRATED
+    else:
+      self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
+
+  def handle_orb_features(self, log):
+    self.update_vp()
+    self.time_orb, frame_raw = self.parse_orb_features(log)
+    self.dt = min(self.time_orb - self.orb_last_updated, 1.)
+    self.orb_last_updated = self.time_orb
+    if self.time_orb - self.l100_last_updated < 1:
+      return self.update(frame_raw, self.yaw_rate, self.speed)
+
+  def handle_live100(self, log):
+    self.l100_last_updated = self.time_orb
+    self.speed = log.live100.vEgo
+    self.angle_offset = log.live100.angleOffset
+    self.yaw_rate = log.live100.curvature * self.speed
+
+  def handle_debug(self):
+    grid_blurred = blur_image(self.grid, self.kernel)
+    grid_grey = np.clip(grid_blurred/(0.1 + np.max(grid_blurred))*255, 0, 255)
+    grid_color = np.repeat(grid_grey[:,:,np.newaxis], 3, axis=2)
+    grid_color[:,:,0] = 0
+    cv2.circle(grid_color, tuple(self.vp.astype(np.int64)), 2, (255, 255, 0), -1)
+    cv2.imshow("debug", grid_color.astype(np.uint8))
+
+    cv2.waitKey(1)
+
+  def send_data(self, livecalibration):
+    calib = get_calib_from_vp(self.vp)
+    extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
+    ke = eon_intrinsics.dot(extrinsic_matrix)
+    warp_matrix = get_camera_frame_from_model_frame(ke, model_height)
+
+    cal_send = messaging.new_message()
+    cal_send.init('liveCalibration')
+    cal_send.liveCalibration.calStatus = self.cal_status
+    cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100)
+    cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten())
+    cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten())
+
+    livecalibration.send(cal_send.to_bytes())
+
+
+def calibrationd_thread(gctx=None, addr="127.0.0.1"):
+  context = zmq.Context()
+
+  live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True)
+  orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True)
+  livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port)
+
+  calibrator = Calibrator(param_put = True)
+
+  # buffer with all the messages that still need to be input into the kalman
+  while 1:
+    of = messaging.recv_one(orbfeatures)
+    l100 = messaging.recv_one_or_none(live100)
+
+    new_vp = calibrator.handle_orb_features(of)
+    if DEBUG and new_vp is not None:
+      print 'got new vp', new_vp
+    if l100 is not None:
+      calibrator.handle_live100(l100)
+    if DEBUG:
+      calibrator.handle_debug()
+
+    calibrator.send_data(livecalibration)
+
+
+def main(gctx=None, addr="127.0.0.1"):
+  calibrationd_thread(gctx, addr)
+
+if __name__ == "__main__":
+  main()
+
--- a/selfdrive/locationd/get_vp.c
+++ b/selfdrive/locationd/get_vp.c
@ -0,0 +1,41 @@
+int get_intersections(double *lines, double *intersections, long long n) {
+  double D, Dx, Dy;
+  double x, y;
+  double *L1, *L2;
+  int k = 0;
+  for (int i=0; i < n; i++) {
+    for (int j=0; j < n; j++) {
+      L1 = lines + i*3;
+      L2 = lines + j*3;
+      D = L1[0] * L2[1] - L1[1] * L2[0];
+      Dx = L1[2] * L2[1] - L1[1] * L2[2];
+      Dy = L1[0] * L2[2] - L1[2] * L2[0];
+      // only intersect lines from different quadrants
+      if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0)){
+        x = Dx / D;
+        y = Dy / D;
+        if ((0 < x) &&
+            (x < W) &&
+            (0 < y) &&
+            (y < H)){
+          intersections[k*2 + 0] = x;
+          intersections[k*2 + 1] = y;
+          k++;
+        }
+      }
+    }
+  }
+  return k;
+}
+
+void increment_grid(double *grid, double *lines, long long n) {
+  double *intersections = (double*) malloc(n*n*2*sizeof(double));
+  int y, x, k;
+  k = get_intersections(lines, intersections, n);
+  for (int i=0; i < k; i++) {
+    x = (int) (intersections[i*2 + 0] + 0.5);
+    y = (int) (intersections[i*2 + 1] + 0.5);
+    grid[y*(W+1) + x] += 1.;
+  }
+  free(intersections);
+}
--- a/selfdrive/loggerd/loggerd
+++ b/selfdrive/loggerd/loggerd
@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9102036c0e855b6c95c8579a7f9b447a981b4dd505a06cfc403eeb319bc74295
+oid sha256:53a9238e3f5c8906cc0d4bbff573040d65a28010a6fc3ac0273030836ec123e7
 size 1626840
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@ -93,9 +93,11 @@ managed_processes = {
  "boardd": ("selfdrive/boardd", ["./boardd"]),   # not used directly
  "pandad": "selfdrive.pandad",
  "ui": ("selfdrive/ui", ["./ui"]),
+  "calibrationd": "selfdrive.locationd.calibrationd",
  "visiond": ("selfdrive/visiond", ["./visiond"]),
  "sensord": ("selfdrive/sensord", ["./sensord"]),
  "gpsd": ("selfdrive/sensord", ["./gpsd"]),
+  "orbd": ("selfdrive/orbd", ["./orbd_wrapper.sh"]),
  "updated": "selfdrive.updated",
 }
 android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
@ -126,9 +128,11 @@ car_started_processes = [
  'loggerd',
  'sensord',
  'radard',
+  'calibrationd',
  'visiond',
  'proclogd',
  'ubloxd',
+  'orbd'
 ]

 def register_managed_process(name, desc, car_started=False):
@ -466,7 +470,7 @@ def main():
  if params.get("IsUploadVideoOverCellularEnabled") is None:
    params.put("IsUploadVideoOverCellularEnabled", "1")
  if params.get("IsDriverMonitoringEnabled") is None:
-    params.put("IsDriverMonitoringEnabled", "0")
+    params.put("IsDriverMonitoringEnabled", "1")
  if params.get("IsGeofenceEnabled") is None:
    params.put("IsGeofenceEnabled", "-1")

--- a/selfdrive/orbd/.gitignore
+++ b/selfdrive/orbd/.gitignore
@ -0,0 +1,8 @@
+orbd
+orbd_cpu
+test/turbocv_profile
+test/turbocv_test
+dspout/*
+dumb_test
+bilinear_lut.h
+orb_lut.h
--- a/selfdrive/orbd/Makefile
+++ b/selfdrive/orbd/Makefile
@ -0,0 +1,105 @@
+# CPU 
+
+CC = clang
+CXX = clang++
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+JSON_FLAGS = -I$(PHONELIBS)/json/src
+
+CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
+CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
+LDFLAGS =
+
+# profile
+# CXXFLAGS += -DTURBOCV_PROFILE=1
+
+PHONELIBS = ../../phonelibs
+BASEDIR = ../..
+EXTERNAL = ../../external
+PYTHONLIBS = 
+
+UNAME_M := $(shell uname -m)
+
+ifeq ($(UNAME_M),x86_64)
+# computer
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
+           -l:libczmq.a -l:libzmq.a -lpthread
+
+OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc
+
+CXXFLAGS += -fopenmp
+LDFLAGS += -lomp
+
+else
+# phone
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+	-l:libczmq.a -l:libzmq.a \
+	-lgnustl_shared
+
+OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include
+OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so
+
+endif
+
+.PHONY: all
+all: orbd
+
+include ../common/cereal.mk
+
+DEP_OBJS = ../common/visionipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
+
+orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+    $(LDFLAGS) \
+		$(ZMQ_LIBS) \
+		$(CEREAL_LIBS) \
+		-L/usr/lib \
+		-L/system/vendor/lib64 \
+		-ladsprpc \
+		-lm -lz -llog
+
+%.o: %.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) \
+		$(ZMQ_FLAGS) \
+		-I../ \
+		-I../../ \
+		-c -o '$@' '$<'
+
+orbd_dsp.o: orbd.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) \
+		$(CEREAL_CXXFLAGS) \
+		$(ZMQ_FLAGS) \
+		$(OPENCV_FLAGS) \
+		-DDSP \
+		-I../ \
+		-I../../ \
+		-I../../../ \
+		-I./include \
+		-c -o '$@' '$<'
+
+freethedsp.o: dsp/freethedsp.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) \
+		-c -o '$@' '$<'
+
+calculator_stub.o: dsp/gen/calculator_stub.c
+	@echo "[ CC ] $@"
+	$(CC) $(CFLAGS) -I./include -c -o '$@' '$<'
+
+-include internal.mk
+
+.PHONY: clean
+clean:
+	rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h
+
--- a/selfdrive/orbd/dsp/freethedsp.c
+++ b/selfdrive/orbd/dsp/freethedsp.c
@ -0,0 +1,119 @@
+// freethedsp by geohot
+//   (because the DSP should be free)
+// released under MIT License
+
+// usage instructions:
+//   1. Compile an example from the Qualcomm Hexagon SDK 
+//   2. Try to run it on your phone
+//   3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat
+// ...here is where people would give up before freethedsp
+//   4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program)
+//   5. Run your program with 'LD_PRELOAD=./freethedsp.so ./<your_prog>'
+//   6. OMG THE DSP WORKS
+//   7. Be happy.
+
+// *** patch may have to change for your phone ***
+
+// this is patching /dsp/fastrpc_shell_0
+// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8"
+// patch to return 0xFFFFFFFF from is_test_enabled instead of 0
+// your fastrpc_shell_0 may vary
+#define PATCH_ADDR 0x5200c
+#define PATCH_OLD "\x40\x3f\x20\x50"
+#define PATCH_NEW "\x40\x3f\x00\x5a"
+#define PATCH_LEN (sizeof(PATCH_OLD)-1)
+#define _BITS_IOCTL_H_
+
+// under 100 lines of code begins now
+#include <stdio.h>
+#include <dlfcn.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+// ioctl stuff
+#define IOC_OUT   0x40000000  /* copy out parameters */
+#define IOC_IN    0x80000000  /* copy in parameters */
+#define IOC_INOUT (IOC_IN|IOC_OUT)
+#define IOCPARM_MASK  0x1fff    /* parameter length, at most 13 bits */
+
+#define _IOC(inout,group,num,len) \
+  (inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num))
+#define _IOWR(g,n,t)  _IOC(IOC_INOUT, (g), (n), sizeof(t))
+
+// ion ioctls
+#include <linux/ion.h>
+#define ION_IOC_MSM_MAGIC 'M'
+#define ION_IOC_CLEAN_INV_CACHES  _IOWR(ION_IOC_MSM_MAGIC, 2, \
+            struct ion_flush_data)
+
+struct ion_flush_data {
+  ion_user_handle_t handle;
+  int fd;
+  void *vaddr;
+  unsigned int offset;
+  unsigned int length;
+};
+
+// fastrpc ioctls
+#define FASTRPC_IOCTL_INIT       _IOWR('R', 6, struct fastrpc_ioctl_init)
+
+struct fastrpc_ioctl_init {
+  uint32_t flags;   /* one of FASTRPC_INIT_* macros */
+  uintptr_t __user file;  /* pointer to elf file */
+  int32_t filelen;  /* elf file length */
+  int32_t filefd;   /* ION fd for the file */
+  uintptr_t __user mem; /* mem for the PD */
+  int32_t memlen;   /* mem length */
+  int32_t memfd;    /* ION fd for the mem */
+};
+
+int ioctl(int fd, unsigned long request, void *arg) {
+  static void *handle = NULL;
+  static int (*orig_ioctl)(int, int, void*);
+
+  if (handle == NULL) {
+    handle = dlopen("/system/lib64/libc.so", RTLD_LAZY);
+    assert(handle != NULL);
+    orig_ioctl = dlsym(handle, "ioctl");
+  }
+
+  int ret = orig_ioctl(fd, request, arg);
+
+  // carefully modify this one
+  if (request == FASTRPC_IOCTL_INIT) {
+    struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg;
+
+    // confirm patch is correct and do the patch
+    assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0);
+    memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN);
+
+    // flush cache
+    int ionfd = open("/dev/ion", O_RDONLY);
+    assert(ionfd > 0);
+
+    struct ion_fd_data fd_data;
+    fd_data.fd = init->memfd;
+    int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data);
+    assert(ret == 0);
+
+    struct ion_flush_data flush_data;
+    flush_data.handle  = fd_data.handle;
+    flush_data.vaddr   = (void*)init->mem;
+    flush_data.offset  = 0;
+    flush_data.length  = init->memlen;
+    ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data);
+    assert(ret == 0);
+
+    struct ion_handle_data handle_data;
+    handle_data.handle = fd_data.handle;
+    ret = ioctl(ionfd, ION_IOC_FREE, &handle_data);
+    assert(ret == 0);
+
+    // cleanup
+    close(ionfd);
+  }
+
+  return ret;
+}
+
--- a/selfdrive/orbd/dsp/gen/calculator.h
+++ b/selfdrive/orbd/dsp/gen/calculator.h
@ -0,0 +1,39 @@
+#ifndef _CALCULATOR_H
+#define _CALCULATOR_H
+
+#include <stdint.h>
+typedef  uint8_t           uint8;
+typedef  uint32_t           uint32;
+
+#ifndef __QAIC_HEADER
+#define __QAIC_HEADER(ff) ff
+#endif //__QAIC_HEADER
+
+#ifndef __QAIC_HEADER_EXPORT
+#define __QAIC_HEADER_EXPORT
+#endif // __QAIC_HEADER_EXPORT
+
+#ifndef __QAIC_HEADER_ATTRIBUTE
+#define __QAIC_HEADER_ATTRIBUTE
+#endif // __QAIC_HEADER_ATTRIBUTE
+
+#ifndef __QAIC_IMPL
+#define __QAIC_IMPL(ff) ff
+#endif //__QAIC_IMPL
+
+#ifndef __QAIC_IMPL_EXPORT
+#define __QAIC_IMPL_EXPORT
+#endif // __QAIC_IMPL_EXPORT
+
+#ifndef __QAIC_IMPL_ATTRIBUTE
+#define __QAIC_IMPL_ATTRIBUTE
+#endif // __QAIC_IMPL_ATTRIBUTE
+#ifdef __cplusplus
+extern "C" {
+#endif
+__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE;
+__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE;
+#ifdef __cplusplus
+}
+#endif
+#endif //_CALCULATOR_H
--- a/selfdrive/orbd/dsp/gen/calculator_stub.c
+++ b/selfdrive/orbd/dsp/gen/calculator_stub.c
@ -0,0 +1,613 @@
+#ifndef _CALCULATOR_STUB_H
+#define _CALCULATOR_STUB_H
+#include "calculator.h"
+
+// remote.h
+#include <stdint.h>
+#include <sys/types.h>
+
+typedef uint32_t remote_handle;
+typedef uint64_t remote_handle64;
+
+typedef struct {
+   void *pv;
+   size_t nLen;
+} remote_buf;
+
+typedef struct {
+   int32_t fd;
+   uint32_t offset;
+} remote_dma_handle;
+
+typedef union {
+   remote_buf     buf;
+   remote_handle    h;
+   remote_handle64 h64;
+   remote_dma_handle dma;
+} remote_arg;
+
+int remote_handle_open(const char* name, remote_handle *ph);
+int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra);
+int remote_handle_close(remote_handle h);
+
+#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \
+          ((((uint32_t)   (nAttr) &  0x7) << 29) | \
+           (((uint32_t) (nMethod) & 0x1f) << 24) | \
+           (((uint32_t)     (nIn) & 0xff) << 16) | \
+           (((uint32_t)    (nOut) & 0xff) <<  8) | \
+           (((uint32_t)    (noIn) & 0x0f) <<  4) | \
+            ((uint32_t)   (noOut) & 0x0f))
+
+#ifndef _QAIC_ENV_H
+#define _QAIC_ENV_H
+
+#ifdef __GNUC__
+#ifdef __clang__
+#pragma GCC diagnostic ignored "-Wunknown-pragmas"
+#else
+#pragma GCC diagnostic ignored "-Wpragmas"
+#endif
+#pragma GCC diagnostic ignored "-Wuninitialized"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wunused-function"
+#endif
+
+#ifndef _ATTRIBUTE_UNUSED
+
+#ifdef _WIN32
+#define _ATTRIBUTE_UNUSED
+#else
+#define _ATTRIBUTE_UNUSED __attribute__ ((unused))
+#endif
+
+#endif // _ATTRIBUTE_UNUSED
+
+#ifndef __QAIC_REMOTE
+#define __QAIC_REMOTE(ff) ff
+#endif //__QAIC_REMOTE
+
+#ifndef __QAIC_HEADER
+#define __QAIC_HEADER(ff) ff
+#endif //__QAIC_HEADER
+
+#ifndef __QAIC_HEADER_EXPORT
+#define __QAIC_HEADER_EXPORT
+#endif // __QAIC_HEADER_EXPORT
+
+#ifndef __QAIC_HEADER_ATTRIBUTE
+#define __QAIC_HEADER_ATTRIBUTE
+#endif // __QAIC_HEADER_ATTRIBUTE
+
+#ifndef __QAIC_IMPL
+#define __QAIC_IMPL(ff) ff
+#endif //__QAIC_IMPL
+
+#ifndef __QAIC_IMPL_EXPORT
+#define __QAIC_IMPL_EXPORT
+#endif // __QAIC_IMPL_EXPORT
+
+#ifndef __QAIC_IMPL_ATTRIBUTE
+#define __QAIC_IMPL_ATTRIBUTE
+#endif // __QAIC_IMPL_ATTRIBUTE
+
+#ifndef __QAIC_STUB
+#define __QAIC_STUB(ff) ff
+#endif //__QAIC_STUB
+
+#ifndef __QAIC_STUB_EXPORT
+#define __QAIC_STUB_EXPORT
+#endif // __QAIC_STUB_EXPORT
+
+#ifndef __QAIC_STUB_ATTRIBUTE
+#define __QAIC_STUB_ATTRIBUTE
+#endif // __QAIC_STUB_ATTRIBUTE
+
+#ifndef __QAIC_SKEL
+#define __QAIC_SKEL(ff) ff
+#endif //__QAIC_SKEL__
+
+#ifndef __QAIC_SKEL_EXPORT
+#define __QAIC_SKEL_EXPORT
+#endif // __QAIC_SKEL_EXPORT
+
+#ifndef __QAIC_SKEL_ATTRIBUTE
+#define __QAIC_SKEL_ATTRIBUTE
+#endif // __QAIC_SKEL_ATTRIBUTE
+
+#ifdef __QAIC_DEBUG__
+   #ifndef __QAIC_DBG_PRINTF__
+   #include <stdio.h>
+   #define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0)
+   #endif
+#else
+   #define __QAIC_DBG_PRINTF__( ee ) (void)0
+#endif
+
+
+#define _OFFSET(src, sof)  ((void*)(((char*)(src)) + (sof)))
+
+#define _COPY(dst, dof, src, sof, sz)  \
+   do {\
+         struct __copy { \
+            char ar[sz]; \
+         };\
+         *(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\
+   } while (0)
+
+#define _COPYIF(dst, dof, src, sof, sz)  \
+   do {\
+      if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\
+         _COPY(dst, dof, src, sof, sz); \
+      } \
+   } while (0)
+
+_ATTRIBUTE_UNUSED
+static __inline void _qaic_memmove(void* dst, void* src, int size) {
+   int i;
+   for(i = 0; i < size; ++i) {
+      ((char*)dst)[i] = ((char*)src)[i];
+   }
+}
+
+#define _MEMMOVEIF(dst, src, sz)  \
+   do {\
+      if(dst != src) {\
+         _qaic_memmove(dst, src, sz);\
+      } \
+   } while (0)
+
+
+#define _ASSIGN(dst, src, sof)  \
+   do {\
+      dst = OFFSET(src, sof); \
+   } while (0)
+
+#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str))
+
+#define AEE_SUCCESS              0
+#define AEE_EOFFSET          0x80000400
+#define AEE_EBADPARM             (AEE_EOFFSET + 0x00E)
+
+#define _TRY(ee, func) \
+   do { \
+      if (AEE_SUCCESS != ((ee) = func)) {\
+         __QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\
+         goto ee##bail;\
+      } \
+   } while (0)
+
+#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS)
+
+#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS)
+
+#ifdef __QAIC_DEBUG__
+#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv))
+#else
+#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv))
+#endif
+
+
+#endif // _QAIC_ENV_H
+
+#ifndef _ALLOCATOR_H
+#define _ALLOCATOR_H
+
+#include <stdlib.h>
+#include <stdint.h>
+
+typedef struct _heap _heap;
+struct _heap {
+   _heap* pPrev;
+   const char* loc;
+   uint64_t buf;
+};
+
+typedef struct _allocator {
+   _heap* pheap;
+   uint8_t* stack;
+   uint8_t* stackEnd;
+   int nSize;
+} _allocator;
+
+_ATTRIBUTE_UNUSED
+static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) {
+   _heap* pn = 0;
+   pn = malloc(size + sizeof(_heap) - sizeof(uint64_t));
+   if(pn != 0) {
+      pn->pPrev = *ppa;
+      pn->loc = loc;
+      *ppa = pn;
+      *ppbuf = (void*)&(pn->buf);
+      return 0;
+   } else {
+      return -1;
+   }
+}
+#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1))
+
+_ATTRIBUTE_UNUSED
+static __inline int _allocator_alloc(_allocator* me,
+                                    const char* loc,
+                                    int size,
+                                    unsigned int al,
+                                    void** ppbuf) {
+   if(size < 0) {
+      return -1;
+   } else if (size == 0) {
+      *ppbuf = 0;
+      return 0;
+   }
+   if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) {
+      *ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al);
+      me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size;
+      return 0;
+   } else {
+      return _heap_alloc(&me->pheap, loc, size, ppbuf);
+   }
+}
+
+_ATTRIBUTE_UNUSED
+static __inline void _allocator_deinit(_allocator* me) {
+   _heap* pa = me->pheap;
+   while(pa != 0) {
+      _heap* pn = pa;
+      const char* loc = pn->loc;
+      (void)loc;
+      pa = pn->pPrev;
+      free(pn);
+   }
+}
+
+_ATTRIBUTE_UNUSED
+static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) {
+   me->stack =  stack;
+   me->stackEnd =  stack + stackSize;
+   me->nSize = stackSize;
+   me->pheap = 0;
+}
+
+
+#endif // _ALLOCATOR_H
+
+#ifndef SLIM_H
+#define SLIM_H
+
+#include <stdint.h>
+
+//a C data structure for the idl types that can be used to implement
+//static and dynamic language bindings fairly efficiently.
+//
+//the goal is to have a minimal ROM and RAM footprint and without
+//doing too many allocations.  A good way to package these things seemed
+//like the module boundary, so all the idls within  one module can share
+//all the type references.
+
+
+#define PARAMETER_IN       0x0
+#define PARAMETER_OUT      0x1
+#define PARAMETER_INOUT    0x2
+#define PARAMETER_ROUT     0x3
+#define PARAMETER_INROUT   0x4
+
+//the types that we get from idl
+#define TYPE_OBJECT             0x0
+#define TYPE_INTERFACE          0x1
+#define TYPE_PRIMITIVE          0x2
+#define TYPE_ENUM               0x3
+#define TYPE_STRING             0x4
+#define TYPE_WSTRING            0x5
+#define TYPE_STRUCTURE          0x6
+#define TYPE_UNION              0x7
+#define TYPE_ARRAY              0x8
+#define TYPE_SEQUENCE           0x9
+
+//these require the pack/unpack to recurse
+//so it's a hint to those languages that can optimize in cases where
+//recursion isn't necessary.
+#define TYPE_COMPLEX_STRUCTURE  (0x10 | TYPE_STRUCTURE)
+#define TYPE_COMPLEX_UNION      (0x10 | TYPE_UNION)
+#define TYPE_COMPLEX_ARRAY      (0x10 | TYPE_ARRAY)
+#define TYPE_COMPLEX_SEQUENCE   (0x10 | TYPE_SEQUENCE)
+
+
+typedef struct Type Type;
+
+#define INHERIT_TYPE\
+   int32_t nativeSize;                /*in the simple case its the same as wire size and alignment*/\
+   union {\
+      struct {\
+         const uintptr_t         p1;\
+         const uintptr_t         p2;\
+      } _cast;\
+      struct {\
+         uint32_t  iid;\
+         uint32_t  bNotNil;\
+      } object;\
+      struct {\
+         const Type  *arrayType;\
+         int32_t      nItems;\
+      } array;\
+      struct {\
+         const Type *seqType;\
+         int32_t      nMaxLen;\
+      } seqSimple; \
+      struct {\
+         uint32_t bFloating;\
+         uint32_t bSigned;\
+      } prim; \
+      const SequenceType* seqComplex;\
+      const UnionType  *unionType;\
+      const StructType *structType;\
+      int32_t         stringMaxLen;\
+      uint8_t        bInterfaceNotNil;\
+   } param;\
+   uint8_t    type;\
+   uint8_t    nativeAlignment\
+
+typedef struct UnionType UnionType;
+typedef struct StructType StructType;
+typedef struct SequenceType SequenceType;
+struct Type {
+   INHERIT_TYPE;
+};
+
+struct SequenceType {
+   const Type *         seqType;
+   uint32_t               nMaxLen;
+   uint32_t               inSize;
+   uint32_t               routSizePrimIn;
+   uint32_t               routSizePrimROut;
+};
+
+//byte offset from the start of the case values for
+//this unions case value array.  it MUST be aligned
+//at the alignment requrements for the descriptor
+//
+//if negative it means that the unions cases are
+//simple enumerators, so the value read from the descriptor
+//can be used directly to find the correct case
+typedef union CaseValuePtr CaseValuePtr;
+union CaseValuePtr {
+   const uint8_t*   value8s;
+   const uint16_t*  value16s;
+   const uint32_t*  value32s;
+   const uint64_t*  value64s;
+};
+
+//these are only used in complex cases
+//so I pulled them out of the type definition as references to make
+//the type smaller
+struct UnionType {
+   const Type           *descriptor;
+   uint32_t               nCases;
+   const CaseValuePtr   caseValues;
+   const Type * const   *cases;
+   int32_t               inSize;
+   int32_t               routSizePrimIn;
+   int32_t               routSizePrimROut;
+   uint8_t                inAlignment;
+   uint8_t                routAlignmentPrimIn;
+   uint8_t                routAlignmentPrimROut;
+   uint8_t                inCaseAlignment;
+   uint8_t                routCaseAlignmentPrimIn;
+   uint8_t                routCaseAlignmentPrimROut;
+   uint8_t                nativeCaseAlignment;
+   uint8_t              bDefaultCase;
+};
+
+struct StructType {
+   uint32_t               nMembers;
+   const Type * const   *members;
+   int32_t               inSize;
+   int32_t               routSizePrimIn;
+   int32_t               routSizePrimROut;
+   uint8_t                inAlignment;
+   uint8_t                routAlignmentPrimIn;
+   uint8_t                routAlignmentPrimROut;
+};
+
+typedef struct Parameter Parameter;
+struct Parameter {
+   INHERIT_TYPE;
+   uint8_t    mode;
+   uint8_t  bNotNil;
+};
+
+#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64))
+#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff)
+
+typedef struct Method Method;
+struct Method {
+   uint32_t                    uScalars;            //no method index
+   int32_t                     primInSize;
+   int32_t                     primROutSize;
+   int                         maxArgs;
+   int                         numParams;
+   const Parameter * const     *params;
+   uint8_t                       primInAlignment;
+   uint8_t                       primROutAlignment;
+};
+
+typedef struct Interface Interface;
+
+struct Interface {
+   int                            nMethods;
+   const Method  * const          *methodArray;
+   int                            nIIds;
+   const uint32_t                   *iids;
+   const uint16_t*                  methodStringArray;
+   const uint16_t*                  methodStrings;
+   const char*                    strings;
+};
+
+
+#endif //SLIM_H
+
+
+#ifndef _CALCULATOR_SLIM_H
+#define _CALCULATOR_SLIM_H
+
+// remote.h
+
+#include <stdint.h>
+
+#ifndef __QAIC_SLIM
+#define __QAIC_SLIM(ff) ff
+#endif
+#ifndef __QAIC_SLIM_EXPORT
+#define __QAIC_SLIM_EXPORT
+#endif
+
+static const Type types[1];
+static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}};
+static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}};
+static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))};
+static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}};
+static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])};
+static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0";
+static const uint16_t methodStrings[5] = {0,37,18,32,27};
+static const uint16_t methodStringsArrays[2] = {3,0};
+__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings};
+#endif //_CALCULATOR_SLIM_H
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef _const_calculator_handle
+#define _const_calculator_handle ((remote_handle)-1)
+#endif //_const_calculator_handle
+
+static void _calculator_pls_dtor(void* data) {
+   remote_handle* ph = (remote_handle*)data;
+   if(_const_calculator_handle != *ph) {
+      (void)__QAIC_REMOTE(remote_handle_close)(*ph);
+      *ph = _const_calculator_handle;
+   }
+}
+
+static int _calculator_pls_ctor(void* ctx, void* data) {
+   remote_handle* ph = (remote_handle*)data;
+   *ph = _const_calculator_handle;
+   if(*ph == (remote_handle)-1) {
+      return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph);
+   }
+   return 0;
+}
+
+#if (defined __qdsp6__) || (defined __hexagon__)
+#pragma weak  adsp_pls_add_lookup
+extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo);
+#pragma weak  HAP_pls_add_lookup
+extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo);
+
+__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) {
+   remote_handle* ph;
+   if(adsp_pls_add_lookup) {
+      if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph),  _calculator_pls_ctor, "calculator",  _calculator_pls_dtor, (void**)&ph))  {
+         return *ph;
+      }
+      return (remote_handle)-1;
+   } else if(HAP_pls_add_lookup) {
+      if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph),  _calculator_pls_ctor, "calculator",  _calculator_pls_dtor, (void**)&ph))  {
+         return *ph;
+      }
+      return (remote_handle)-1;
+   }
+   return(remote_handle)-1;
+}
+
+#else //__qdsp6__ || __hexagon__
+
+uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare);
+
+#ifdef _WIN32
+#include "Windows.h"
+uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) {
+   return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare);
+}
+#elif __GNUC__
+uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) {
+   return __sync_val_compare_and_swap(puDest, uCompare, uExchange);
+}
+#endif //_WIN32
+
+
+__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) {
+   static remote_handle handle = _const_calculator_handle;
+   if((remote_handle)-1 != handle) {
+      return handle;
+   } else {
+      remote_handle tmp;
+      int nErr = _calculator_pls_ctor("calculator", (void*)&tmp);
+      if(nErr) {
+         return (remote_handle)-1;
+      }
+      if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) {
+         _calculator_pls_dtor(&tmp);
+      }
+      return handle;
+   }
+}
+
+#endif //__qdsp6__
+
+__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE {
+   return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern int remote_register_dma_handle(int, uint32_t);
+static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) {
+   int _numIn[1];
+   remote_arg _pra[1];
+   uint32_t _primROut[1];
+   int _nErr = 0;
+   _numIn[0] = 0;
+   _pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut;
+   _pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut);
+   _TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra));
+   _COPY(_rout0, 0, _primROut, 0, 4);
+   _CATCH(_nErr) {}
+   return _nErr;
+}
+__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE {
+   uint32_t _mid = 0;
+   return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet);
+}
+static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) {
+   int _numIn[1];
+   remote_arg _pra[3];
+   uint32_t _primIn[2];
+   remote_arg* _praIn;
+   remote_arg* _praROut;
+   int _nErr = 0;
+   _numIn[0] = 1;
+   _pra[0].buf.pv = (void*)_primIn;
+   _pra[0].buf.nLen = sizeof(_primIn);
+   _COPY(_primIn, 0, _in0Len, 0, 4);
+   _praIn = (_pra + 1);
+   _praIn[0].buf.pv = _in0[0];
+   _praIn[0].buf.nLen = (1 * _in0Len[0]);
+   _COPY(_primIn, 4, _rout1Len, 0, 4);
+   _praROut = (_praIn + _numIn[0] + 0);
+   _praROut[0].buf.pv = _rout1[0];
+   _praROut[0].buf.nLen = (1 * _rout1Len[0]);
+   _TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra));
+   _CATCH(_nErr) {}
+   return _nErr;
+}
+__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE {
+   uint32_t _mid = 1;
+   return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen);
+}
+#ifdef __cplusplus
+}
+#endif
+#endif //_CALCULATOR_STUB_H
--- a/selfdrive/orbd/dsp/gen/libcalculator_skel.so
+++ b/selfdrive/orbd/dsp/gen/libcalculator_skel.so
@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b94bcedb39d110b074a99fe3129da2506bc4b06cca7f84731cd0620683fd1179
+size 357400
--- a/selfdrive/orbd/extractor.h
+++ b/selfdrive/orbd/extractor.h
@ -0,0 +1,38 @@
+#ifndef EXTRACTOR_H
+#define EXTRACTOR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#define ORBD_KEYPOINTS 3000
+#define ORBD_DESCRIPTOR_LENGTH 32
+#define ORBD_HEIGHT 874
+#define ORBD_WIDTH 1164
+#define ORBD_FOCAL 910
+
+// matches OrbFeatures from log.capnp
+struct orb_features {
+  // align this
+  uint16_t n_corners;
+  uint16_t xy[ORBD_KEYPOINTS][2];
+  uint8_t octave[ORBD_KEYPOINTS];
+  uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH];
+  int16_t matches[ORBD_KEYPOINTS];
+};
+
+// forward declare this
+struct pyramid;
+
+// manage the pyramids in extractor.c
+void init_gpyrs();
+int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *);
+int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // EXTRACTOR_H
--- a/selfdrive/orbd/orbd.cc
+++ b/selfdrive/orbd/orbd.cc
@ -0,0 +1,191 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <assert.h>
+#include <sys/resource.h>
+
+#include "common/visionipc.h"
+#include "common/swaglog.h"
+
+#include "extractor.h"
+
+#ifdef DSP
+#include "dsp/gen/calculator.h"
+#else
+#include "turbocv.h"
+#endif
+
+#include <zmq.h>
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#ifndef PATH_MAX
+#include <linux/limits.h>
+#endif
+
+volatile int do_exit = 0;
+
+static void set_do_exit(int sig) {
+  do_exit = 1;
+}
+
+int main(int argc, char *argv[]) {
+  int err;
+  setpriority(PRIO_PROCESS, 0, -13);
+  printf("starting orbd\n");
+
+#ifdef DSP
+  uint32_t test_leet = 0;
+  char my_path[PATH_MAX+1];
+  memset(my_path, 0, sizeof(my_path));
+
+  ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path));
+  assert(len > 5);
+  my_path[len-5] = '\0';
+  LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX);
+
+  char adsp_path[PATH_MAX+1];
+  snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path);
+  assert(putenv(adsp_path) == 0);
+
+  assert(calculator_init(&test_leet) == 0);
+  assert(test_leet == 0x1337);
+  LOGW("orbd init complete");
+#else
+  init_gpyrs();
+#endif
+
+  signal(SIGINT, (sighandler_t) set_do_exit);
+  signal(SIGTERM, (sighandler_t) set_do_exit);
+
+  void *ctx = zmq_ctx_new();
+
+  void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB);
+  assert(orb_features_sock);
+  zmq_bind(orb_features_sock, "tcp://*:8058");
+
+  void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB);
+  assert(orb_features_summary_sock);
+  zmq_bind(orb_features_summary_sock, "tcp://*:8062");
+
+  struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features));
+  int last_frame_id = 0;
+  uint64_t frame_count = 0;
+
+  // every other frame
+  const int RATE = 2;
+
+  VisionStream stream;
+  while (!do_exit) {
+    VisionStreamBufs buf_info;
+    err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info);
+    if (err) {
+      printf("visionstream connect fail\n");
+      usleep(100000);
+      continue;
+    }
+    uint64_t timestamp_last_eof = 0;
+    while (!do_exit) {
+      VIPCBuf *buf;
+      VIPCBufExtra extra;
+      buf = visionstream_get(&stream, &extra);
+      if (buf == NULL) {
+        printf("visionstream get failed\n");
+        break;
+      }
+
+      // every other frame
+      frame_count++;
+      if ((frame_count%RATE) != 0) {
+        continue;
+      }
+
+      uint64_t start = nanos_since_boot();
+#ifdef DSP
+      int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features));
+#else
+      int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features);
+#endif
+      uint64_t end = nanos_since_boot();
+      LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id);
+      assert(ret == 0);
+
+      if (last_frame_id+RATE != extra.frame_id) {
+        LOGW("dropped frame!");
+      }
+
+      last_frame_id = extra.frame_id;
+
+      if (timestamp_last_eof == 0) {
+        timestamp_last_eof = extra.timestamp_eof;
+        continue;
+      }
+
+      int match_count = 0;
+
+      // *** send OrbFeatures ***
+      {
+        // create capnp message
+        capnp::MallocMessageBuilder msg;
+        cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+        event.setLogMonoTime(nanos_since_boot());
+
+        auto orb_features = event.initOrbFeatures();
+
+        // set timestamps
+        orb_features.setTimestampEof(extra.timestamp_eof);
+        orb_features.setTimestampLastEof(timestamp_last_eof);
+
+        // init descriptors for send
+        kj::ArrayPtr<capnp::byte> descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners);
+        orb_features.setDescriptors(descriptorsPtr);
+
+        auto xs = orb_features.initXs(features->n_corners);
+        auto ys = orb_features.initYs(features->n_corners);
+        auto octaves = orb_features.initOctaves(features->n_corners);
+        auto matches = orb_features.initMatches(features->n_corners);
+
+        // copy out normalized keypoints
+        for (int i = 0; i < features->n_corners; i++) {
+          xs.set(i, (features->xy[i][0] * 1.0f - ORBD_WIDTH / 2) / ORBD_FOCAL);
+          ys.set(i, (features->xy[i][1] * 1.0f - ORBD_HEIGHT / 2) / ORBD_FOCAL);
+          octaves.set(i, features->octave[i]);
+          matches.set(i, features->matches[i]);
+          match_count += features->matches[i] != -1;
+        }
+
+        auto words = capnp::messageToFlatArray(msg);
+        auto bytes = words.asBytes();
+        zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0);
+      }
+
+      // *** send OrbFeaturesSummary ***
+
+      {
+        // create capnp message
+        capnp::MallocMessageBuilder msg;
+        cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+        event.setLogMonoTime(nanos_since_boot());
+
+        auto orb_features_summary = event.initOrbFeaturesSummary();
+
+        orb_features_summary.setTimestampEof(extra.timestamp_eof);
+        orb_features_summary.setTimestampLastEof(timestamp_last_eof);
+        orb_features_summary.setFeatureCount(features->n_corners);
+        orb_features_summary.setMatchCount(match_count);
+        orb_features_summary.setComputeNs(end-start);
+
+        auto words = capnp::messageToFlatArray(msg);
+        auto bytes = words.asBytes();
+        zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0);
+      }
+
+      timestamp_last_eof = extra.timestamp_eof;
+    }
+  }
+  visionstream_destroy(&stream);
+  return 0;
+}
+
--- a/selfdrive/orbd/orbd_wrapper.sh
+++ b/selfdrive/orbd/orbd_wrapper.sh
@ -0,0 +1,13 @@
+#!/bin/sh
+finish() {
+  echo "exiting orbd"
+  pkill -SIGINT -P $$
+}
+
+trap finish EXIT
+
+while true; do
+  ./orbd &
+  wait $!
+done
+
--- a/selfdrive/sensord/gpsd
+++ b/selfdrive/sensord/gpsd
@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9ecc73ffc43883c4407af3c88678262bc0374fb057814fc8b2c09e56619cbe15
+oid sha256:7fbdea9a1cf92975aa4b7506e93dde1e9b8d87f6c2426f98aa7d8831726fb94e
 size 1171544
--- a/selfdrive/sensord/sensord
+++ b/selfdrive/sensord/sensord
@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9368e1101d7bcf298761955ae0577aa98235d2f4775278d79d591b4bdcb9f249
+oid sha256:35db92ee0ee547a9bdf8260893abed58cd079ca43d3978a18a16def2a6b66aab
 size 1159016
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@ -91,8 +91,16 @@ const int alert_sizes[] = {
  [ALERTSIZE_FULL] = vwp_h,
 };

+// TODO: this is also hardcoded in common/transformations/camera.py
+const mat3 intrinsic_matrix = (mat3){{
+  910., 0., 582.,
+  0., 910., 437.,
+  0.,   0.,   1.
+}};
+
 typedef struct UIScene {
  int frontview;
+  int fullview;

  int transformed_width, transformed_height;

@ -211,9 +219,6 @@ typedef struct UIState {
  unsigned int rgb_front_width, rgb_front_height, rgb_front_stride;
  size_t rgb_front_buf_len;

-  bool intrinsic_matrix_loaded;
-  mat3 intrinsic_matrix;
-
  UIScene scene;

  bool awake;
@ -318,6 +323,14 @@ static const mat4 frame_transform = {{
                                           0.0, 0.0, 0.0, 1.0,
 }};

+// frame from 4/3 to 16/9 display
+static const mat4 full_to_wide_frame_transform = {{
+  .75,  0.0, 0.0, 0.0,
+  0.0,  1.0, 0.0, 0.0,
+  0.0,  0.0, 1.0, 0.0,
+  0.0,  0.0, 0.0, 1.0,
+}};
+
 static void ui_init(UIState *s) {
  memset(s, 0, sizeof(UIState));

@ -419,37 +432,6 @@ static void ui_init(UIState *s) {
  }
 }

-// If the intrinsics are in the params entry, this copies them to
-// intrinsic_matrix and returns true.  Otherwise returns false.
-static bool try_load_intrinsics(mat3 *intrinsic_matrix) {
-  char *value;
-  const int result = read_db_value(NULL, "CloudCalibration", &value, NULL);
-
-  if (result == 0) {
-    JsonNode* calibration_json = json_decode(value);
-    free(value);
-
-    JsonNode *intrinsic_json =
-        json_find_member(calibration_json, "intrinsic_matrix");
-
-    if (intrinsic_json == NULL || intrinsic_json->tag != JSON_ARRAY) {
-      json_delete(calibration_json);
-      return false;
-    }
-
-    int i = 0;
-    JsonNode* json_num;
-    json_foreach(json_num, intrinsic_json) {
-      intrinsic_matrix->v[i++] = json_num->number_;
-    }
-    json_delete(calibration_json);
-
-    return true;
-  } else {
-    return false;
-  }
-}
-
 static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
                           int num_back_fds, const int *back_fds,
                           const VisionStreamBufs front_bufs, int num_front_fds,
@ -467,6 +449,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,

  s->scene = (UIScene){
      .frontview = getenv("FRONTVIEW") != NULL,
+      .fullview = getenv("FULLVIEW") != NULL,
      .cal_status = CALIBRATION_CALIBRATED,
      .transformed_width = ui_info.transformed_width,
      .transformed_height = ui_info.transformed_height,
@ -551,7 +534,7 @@ vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {

  // The last entry is zero because of how we store E (to use matvecmul).
  const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}};
-  const vec3 KEp = matvecmul3(s->intrinsic_matrix, Ep);
+  const vec3 KEp = matvecmul3(intrinsic_matrix, Ep);

  // Project.
  const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}};
@ -773,23 +756,27 @@ static void draw_steering(UIState *s, float curvature) {
 static void draw_frame(UIState *s) {
  const UIScene *scene = &s->scene;

-  mat4 out_mat;
  float x1, x2, y1, y2;
  if (s->scene.frontview) {
-    out_mat = device_transform; // full 16/9
    // flip horizontally so it looks like a mirror
-    x1 = (float)scene->front_box_x / s->rgb_front_width;
-    x2 = (float)(scene->front_box_x + scene->front_box_width) / s->rgb_front_width;
-    y2 = (float)scene->front_box_y / s->rgb_front_height;
-    y1 = (float)(scene->front_box_y + scene->front_box_height) / s->rgb_front_height;
+    x1 = 0.0;
+    x2 = 1.0;
+    y1 = 1.0;
+    y2 = 0.0;
  } else {
-    out_mat = matmul(device_transform, frame_transform);
    x1 = 1.0;
    x2 = 0.0;
    y1 = 1.0;
    y2 = 0.0;
  }

+  mat4 out_mat;
+  if (s->scene.frontview || s->scene.fullview) {
+    out_mat = matmul(device_transform, full_to_wide_frame_transform);
+  } else {
+    out_mat = matmul(device_transform, frame_transform);
+  }
+
  const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
  const float frame_coords[4][4] = {
    {-1.0, -1.0, x2, y1}, //bl
@ -930,7 +917,7 @@ static void ui_draw_vision_speed(UIState *s) {
  if (s->is_metric) {
    snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5));
  } else {
-    snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2374144 + 0.5));
+    snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2369363 + 0.5));
  }
  nvgFontFace(s->vg, "sans-bold");
  nvgFontSize(s->vg, 96*2.5);
@ -1103,7 +1090,7 @@ static void ui_draw_calibration_status(UIState *s) {
  char calib_str1[64];
  char calib_str2[64];
  snprintf(calib_str1, sizeof(calib_str1), "Calibration in Progress: %d%%", scene->cal_perc);
-  snprintf(calib_str2, sizeof(calib_str2), (s->is_metric?"Drive above 72 km/h":"Drive above 45 mph"));
+  snprintf(calib_str2, sizeof(calib_str2), (s->is_metric?"Drive above 35 km/h":"Drive above 15 mph"));

  ui_draw_vision_alert(s, ALERTSIZE_MID, s->status, calib_str1, calib_str2);
 }
@ -1137,7 +1124,7 @@ static void ui_draw_vision(UIState *s) {
  nvgScissor(s->vg, ui_viz_rx, box_y, ui_viz_rw, box_h);
  nvgTranslate(s->vg, ui_viz_rx+ui_viz_ro, box_y + (box_h-inner_height)/2.0);
  nvgScale(s->vg, (float)viz_w / s->fb_w, (float)inner_height / s->fb_h);
-  if (!scene->frontview) {
+  if (!scene->frontview && !scene->fullview) {
    ui_draw_world(s);
  }

@ -1236,10 +1223,6 @@ static void update_status(UIState *s, int status) {
 static void ui_update(UIState *s) {
  int err;

-  if (!s->intrinsic_matrix_loaded) {
-    s->intrinsic_matrix_loaded = try_load_intrinsics(&s->intrinsic_matrix);
-  }
-
  if (s->vision_connect_firstrun) {
    // cant run this in connector thread because opengl.
    // do this here for now in lieu of a run_on_main_thread event
@ -1513,7 +1496,7 @@ static void ui_update(UIState *s) {
        s->scene.lead_y_rel = leaddatad.yRel;
        s->scene.lead_v_rel = leaddatad.vRel;
      } else if (eventd.which == cereal_Event_liveCalibration) {
-        s->scene.world_objects_visible = s->intrinsic_matrix_loaded;
+        s->scene.world_objects_visible = true;
        struct cereal_LiveCalibrationData datad;
        cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);

@ -1736,6 +1719,21 @@ static void* bg_thread(void* args) {
  return NULL;
 }

+int is_leon() {
+  #define MAXCHAR 1000
+  FILE *fp;
+  char str[MAXCHAR];
+  char* filename = "/proc/cmdline";
+
+  fp = fopen(filename, "r");
+  if (fp == NULL){
+    printf("Could not open file %s",filename);
+    return 0;
+  }
+  fgets(str, MAXCHAR, fp);
+  fclose(fp);
+  return strstr(str, "letv") != NULL;
+}

 int main() {
  int err;
@ -1767,14 +1765,14 @@ int main() {
  touch_init(&touch);

  // light sensor scaling params
-  #define LIGHT_SENSOR_M 1.3
-  #define LIGHT_SENSOR_B 5.0
+  const int EON = (access("/EON", F_OK) != -1);
+  const int LEON = is_leon();

+  const float BRIGHTNESS_B = LEON? 10.0 : 5.0;
+  const float BRIGHTNESS_M = LEON? 2.6 : 1.3;
  #define NEO_BRIGHTNESS 100

-  float smooth_light_sensor = LIGHT_SENSOR_B;
-
-  const int EON = (access("/EON", F_OK) != -1);
+  float smooth_brightness = BRIGHTNESS_B;

  while (!do_exit) {
    bool should_swap = false;
@ -1783,10 +1781,10 @@ int main() {
    if (EON) {
      // light sensor is only exposed on EONs

-      float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B;
-      if (clipped_light_sensor > 255) clipped_light_sensor = 255;
-      smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99;
-      set_brightness(s, (int)smooth_light_sensor);
+      float clipped_brightness = (s->light_sensor*BRIGHTNESS_M) + BRIGHTNESS_B;
+      if (clipped_brightness > 255) clipped_brightness = 255;
+      smooth_brightness = clipped_brightness * 0.01 + smooth_brightness * 0.99;
+      set_brightness(s, (int)smooth_brightness);
    } else {
      // compromise for bright and dark envs
      set_brightness(s, NEO_BRIGHTNESS);
--- a/selfdrive/visiond/visiond
+++ b/selfdrive/visiond/visiond
@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:97110fee2fdc891137a643a7eab38bd269f472658ac05e950fe6dceab306822c
-size 12539240
+oid sha256:5f1db707acfb0ef06a15433692d8214695b97ec66ef36a63f74d6b16095a8e35
+size 8775896