diff --git a/.gitignore b/.gitignore
index 71ca33c65..70ad45c32 100644
--- a/.gitignore
+++ b/.gitignore
@@ -20,8 +20,6 @@ a.out
*.class
*.pyxbldc
*.vcd
-lane.cpp
-loc*.cpp
config.json
clcache
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 2357509ee..a65360ebe 100644
--- 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)
diff --git a/README.md b/README.md
index c8762963c..d7f147c58 100644
--- 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 | 25mph1| 12mph |
-| Honda | CR-V 2016 | Honda Sensing | Yes | Yes | 25mph1| 12mph |
+| Honda | Civic 2018 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
+| Honda | CR-V 2015 | Touring | Yes | Yes | 25mph1| 12mph |
+| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph1| 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 | 25mph1| 12mph |
-| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph |
+| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph1| 0mph |
+| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph1| 0mph |
| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph |
| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph |
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph1| 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
------
diff --git a/RELEASES.md b/RELEASES.md
index 0a81ac30e..1f822b6f6 100644
--- 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
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 65ec3ee95..c14ed5b99 100644
--- 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
diff --git a/cereal/log.capnp b/cereal/log.capnp
index fbb00a2a1..06392b88a 100644
--- 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);
}
diff --git a/common/dbc.py b/common/dbc.py
index 103030949..7135b07cf 100755
--- 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)
diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py
new file mode 100644
index 000000000..264dbd7fc
--- /dev/null
+++ 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
diff --git a/common/params.py b/common/params.py
index 9d1b096af..3981a341f 100755
--- 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,
@@ -266,23 +263,50 @@ class DBWriter(DBAccessor):
self._lock = None
+def read_db(params_path, key):
+ path = "%s/d/%s" % (params_path, key)
+ try:
+ with open(path, "rb") as f:
+ return f.read()
+ except IOError:
+ return None
-class JSDB(object):
- def __init__(self, fn):
- self._fn = fn
+def write_db(params_path, key, value):
+ prev_umask = os.umask(0)
+ lock = FileLock(params_path+"/.lock", True)
+ lock.acquire()
- def begin(self, write=False):
- if write:
- return DBWriter(self._fn)
- else:
- return DBReader(self._fn)
+ try:
+ tmp_path = tempfile.mktemp(prefix=".tmp", dir=params_path)
+ with open(tmp_path, "wb") as f:
+ f.write(value)
+ f.flush()
+ os.fsync(f.fileno())
+
+ path = "%s/d/%s" % (params_path, key)
+ os.rename(tmp_path, path)
+ fsync_dir(os.path.dirname(path))
+ finally:
+ os.umask(prev_umask)
+ lock.release()
class Params(object):
def __init__(self, db='/data/params'):
- self.env = JSDB(db)
+ self.db = db
+
+ # create the database if it doesn't exist...
+ if not os.path.exists(self.db+"/d"):
+ with self.transaction(write=True):
+ pass
+
+ def transaction(self, write=False):
+ if write:
+ return DBWriter(self.db)
+ else:
+ return DBReader(self.db)
def _clear_keys_with_type(self, tx_type):
- with self.env.begin(write=True) as txn:
+ with self.transaction(write=True) as txn:
for key in keys:
if keys[key] == tx_type:
txn.delete(key)
@@ -294,7 +318,7 @@ class Params(object):
self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START)
def delete(self, key):
- with self.env.begin(write=True) as txn:
+ with self.transaction(write=True) as txn:
txn.delete(key)
def get(self, key, block=False):
@@ -302,8 +326,7 @@ class Params(object):
raise UnknownKeyName(key)
while 1:
- with self.env.begin() as txn:
- ret = txn.get(key)
+ ret = read_db(self.db, key)
if not block or ret is not None:
break
# is polling really the best we can do?
@@ -314,8 +337,7 @@ class Params(object):
if key not in keys:
raise UnknownKeyName(key)
- with self.env.begin(write=True) as txn:
- txn.put(key, dat)
+ write_db(self.db, key, dat)
if __name__ == "__main__":
params = Params()
diff --git a/common/transformations/camera.py b/common/transformations/camera.py
new file mode 100644
index 000000000..e7472f533
--- /dev/null
+++ 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]
+
diff --git a/common/transformations/model.py b/common/transformations/model.py
new file mode 100644
index 000000000..1a8122d6e
--- /dev/null
+++ 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
diff --git a/common/transformations/orientation.py b/common/transformations/orientation.py
new file mode 100644
index 000000000..9e4edd456
--- /dev/null
+++ 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)))
diff --git a/opendbc/acura_ilx_2016_can_generated.dbc b/opendbc/acura_ilx_2016_can_generated.dbc
index 9b349b980..b7214ff60 100644
--- a/opendbc/acura_ilx_2016_can_generated.dbc
+++ b/opendbc/acura_ilx_2016_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/acura_rdx_2018_can_generated.dbc b/opendbc/acura_rdx_2018_can_generated.dbc
index ec2ecf59a..bbfa1aeac 100644
--- a/opendbc/acura_rdx_2018_can_generated.dbc
+++ b/opendbc/acura_rdx_2018_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/generator/honda/_bosch_2018.dbc b/opendbc/generator/honda/_bosch_2018.dbc
index 9c0ecd717..5d07e3bab 100644
--- a/opendbc/generator/honda/_bosch_2018.dbc
+++ b/opendbc/generator/honda/_bosch_2018.dbc
@@ -64,10 +64,10 @@ BO_ 330 STEERING_SENSORS: 8 EPS
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
BO_ 344 ENGINE_DATA: 8 PCM
- SG_ XMISSION_SPEED : 7|16@0+ (0.002759506,0) [0|70] "m/s" EON
+ SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
- SG_ XMISSION_SPEED2 : 39|16@0+ (0.002759506,0) [0|70] "m/s" EON
- SG_ DISTANCE_COUNTER : 55|8@0+ (10,0) [0|2550] "Meters" XXX
+ SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
diff --git a/opendbc/generator/honda/_honda_2017.dbc b/opendbc/generator/honda/_honda_2017.dbc
index 999cc5dd9..82818f142 100644
--- a/opendbc/generator/honda/_honda_2017.dbc
+++ b/opendbc/generator/honda/_honda_2017.dbc
@@ -37,9 +37,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/generator/honda/honda_accord_lx15t_2018_can.dbc b/opendbc/generator/honda/honda_accord_lx15t_2018_can.dbc
new file mode 100644
index 000000000..9e95f56db
--- /dev/null
+++ b/opendbc/generator/honda/honda_accord_lx15t_2018_can.dbc
@@ -0,0 +1,52 @@
+CM_ "IMPORT _bosch_2018.dbc"
+
+BO_ 401 GEARBOX: 8 PCM
+ SG_ GEAR_SHIFTER : 5|6@0+ (1,0) [0|63] "" EON
+ SG_ BOH : 45|6@0+ (1,0) [0|63] "" XXX
+ SG_ GEAR2 : 31|8@0+ (1,0) [0|1] "" XXX
+ SG_ GEAR : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ ZEROS_BOH : 47|2@0+ (1,0) [0|3] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 432 STANDSTILL: 7 VSA
+ SG_ BRAKE_ERROR_1 : 11|1@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_ERROR_2 : 9|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON
+
+BO_ 446 BRAKE_MODULE: 3 VSA
+ SG_ BRAKE_PRESSED : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 927 RADAR_HUD: 8 RADAR
+ SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY
+ SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY
+ SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY
+ SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX
+ SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY
+ SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX
+ SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ BOH : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ BOH_2 : 30|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 1302 ODOMETER: 8 XXX
+ SG_ ODOMETER : 7|24@0+ (1,0) [0|16777215] "km" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P" ;
+VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ;
+VAL_ 545 ECON_ON_2 0 "off" 3 "on" ;
+VAL_ 662 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ;
+VAL_ 662 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
+VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
+VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
+VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
+
+CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_accord_lx15t_2018_can_generated.dbc b/opendbc/honda_accord_lx15t_2018_can_generated.dbc
new file mode 100644
index 000000000..8cc11ba6a
--- /dev/null
+++ b/opendbc/honda_accord_lx15t_2018_can_generated.dbc
@@ -0,0 +1,319 @@
+CM_ "AUTOGENERATED FILE, DO NOT EDIT"
+
+
+CM_ "Imported file _bosch_2018.dbc starts here"
+VERSION ""
+
+
+NS_ :
+ NS_DESC_
+ CM_
+ BA_DEF_
+ BA_
+ VAL_
+ CAT_DEF_
+ CAT_
+ FILTER
+ BA_DEF_DEF_
+ EV_DATA_
+ ENVVAR_DATA_
+ SGTYPE_
+ SGTYPE_VAL_
+ BA_DEF_SGTYPE_
+ BA_SGTYPE_
+ SIG_TYPE_REF_
+ VAL_TABLE_
+ SIG_GROUP_
+ SIG_VALTYPE_
+ SIGTYPE_VALTYPE_
+ BO_TX_BU_
+ BA_DEF_REL_
+ BA_REL_
+ BA_DEF_DEF_REL_
+ BU_SG_REL_
+ BU_EV_REL_
+ BU_BO_REL_
+ SG_MUL_VAL_
+
+BU_: EBCM EON CAM RADAR PCM EPS VSA SCM BDY XXX EPB
+
+BO_ 228 STEERING_CONTROL: 5 EON
+ SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS
+ SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS
+ SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS
+ SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS
+ SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS
+ SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS
+
+BO_ 232 BRAKE_HOLD: 7 XXX
+ SG_ XMISSION_SPEED : 7|14@0- (1,0) [1|0] "" XXX
+ SG_ COMPUTER_BRAKE : 39|16@0+ (1,0) [0|0] "" XXX
+ SG_ COMPUTER_BRAKE_REQUEST : 29|1@0+ (1,0) [0|0] "" XXX
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 304 GAS_PEDAL_2: 8 PCM
+ SG_ ENGINE_TORQUE_ESTIMATE : 7|16@0- (1,0) [-1000|1000] "Nm" EON
+ SG_ ENGINE_TORQUE_REQUEST : 23|16@0- (1,0) [-1000|1000] "Nm" EON
+ SG_ CAR_GAS : 39|8@0+ (1,0) [0|255] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 330 STEERING_SENSORS: 8 EPS
+ SG_ STEER_ANGLE : 7|16@0- (-0.1,0) [-500|500] "deg" EON
+ SG_ STEER_ANGLE_RATE : 23|16@0- (-1,0) [-3000|3000] "deg/s" EON
+ SG_ STEER_ANGLE_OFFSET : 39|8@0- (-0.1,0) [-128|127] "deg" EON
+ SG_ STEER_WHEEL_ANGLE : 47|16@0- (-0.1,0) [-500|500] "deg" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 344 ENGINE_DATA: 8 PCM
+ SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
+ SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 380 POWERTRAIN_DATA: 8 PCM
+ SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
+ SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
+ SG_ GAS_PRESSED : 39|1@0+ (1,0) [0|1] "" EON
+ SG_ ACC_STATUS : 38|1@0+ (1,0) [0|1] "" EON
+ SG_ BOH_17C : 37|5@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_SWITCH : 32|1@0+ (1,0) [0|1] "" EON
+ SG_ BOH2_17C : 47|10@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_PRESSED : 53|1@0+ (1,0) [0|1] "" EON
+ SG_ BOH3_17C : 52|5@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 399 STEER_STATUS: 7 EPS
+ SG_ STEER_TORQUE_SENSOR : 7|16@0- (1,0) [-31000|31000] "tbd" EON
+ SG_ STEER_TORQUE_MOTOR : 23|16@0- (1,0) [-31000|31000] "tbd" EON
+ SG_ STEER_STATUS : 39|4@0+ (1,0) [0|15] "" EON
+ SG_ STEER_CONTROL_ACTIVE : 35|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON
+
+BO_ 420 VSA_STATUS: 8 VSA
+ SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON
+ SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON
+ SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 450 EPB_STATUS: 8 EPB
+ SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" EON
+ SG_ EPB_STATE : 29|2@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 464 WHEEL_SPEEDS: 8 VSA
+ SG_ WHEEL_SPEED_FL : 7|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ WHEEL_SPEED_FR : 8|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ WHEEL_SPEED_RL : 25|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ WHEEL_SPEED_RR : 42|15@0+ (0.01,0) [0|250] "kph" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+BO_ 479 ACC_CONTROL: 8 EON
+ SG_ SET_TO_1 : 20|5@0+ (1,0) [0|1] "" PCM
+ SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX
+ SG_ RELATED_TO_GAS : 7|7@0+ (1,0) [0|69] "" XXX
+ SG_ GAS_COMMAND : 0|9@0+ (1,0) [0|1] "" PCM
+ SG_ GAS_BRAKE : 31|14@0- (1,0) [0|1] "" XXX
+ SG_ ZEROS_BOH : 33|18@0+ (1,0) [100|100] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 495 ACC_CONTROL_ON: 8 XXX
+ SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_TO_30 : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ ZEROS_BOH : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ ZEROS_BOH2 : 47|16@0+ (1,0) [0|255] "" XXX
+ SG_ SET_TO_FF : 15|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_TO_3 : 6|7@0+ (1,0) [0|4095] "" XXX
+ SG_ CONTROL_ON : 7|1@0+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+
+BO_ 545 XXX_16: 6 SCM
+ SG_ ECON_ON : 23|1@0+ (1,0) [0|1] "" XXX
+ SG_ DRIVE_MODE : 37|2@0+ (1,0) [0|3] "" XXX
+ SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY
+ SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY
+
+BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
+ SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
+ SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
+ SG_ WHEEL_SPEED_RL : 23|8@0+ (1,0) [0|255] "mph" EON
+ SG_ WHEEL_SPEED_RR : 31|8@0+ (1,0) [0|255] "mph" EON
+ SG_ SET_TO_X55 : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ SET_TO_X55_2 : 47|8@0+ (1,0) [0|255] "" EON
+ SG_ LONG_COUNTER : 55|8@0+ (1,0) [0|255] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+
+ BO_ 662 SCM_BUTTONS: 4 SCM
+ SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON
+ SG_ CRUISE_SETTING : 3|2@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 29|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 27|4@0+ (1,0) [0|15] "" EON
+
+BO_ 773 SEATBELT_STATUS: 7 BDY
+ SG_ SEATBELT_DRIVER_LAMP : 7|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_PASS_UNLATCHED : 10|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_PASS_LATCHED : 11|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_DRIVER_UNLATCHED : 12|1@0+ (1,0) [0|1] "" EON
+ SG_ SEATBELT_DRIVER_LATCHED : 13|1@0+ (1,0) [0|1] "" EON
+ SG_ PASS_AIRBAG_OFF : 14|1@0+ (1,0) [0|1] "" EON
+ SG_ PASS_AIRBAG_ON : 15|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON
+
+BO_ 777 CAR_SPEED: 8 PCM
+ SG_ ROUGH_CAR_SPEED : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ CAR_SPEED : 7|16@0+ (1,0) [0|65535] "" XXX
+ SG_ ROUGH_CAR_SPEED_3 : 39|16@0+ (1,0) [0|65535] "" XXX
+ SG_ ROUGH_CAR_SPEED_2 : 31|8@0+ (1,0) [0|255] "" XXX
+ SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 780 ACC_HUD: 8 ADAS
+ SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "" BDY
+ SG_ DTC_MODE : 39|1@0+ (1,0) [0|1] "" BDY
+ SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY
+ SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY
+ SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY
+ SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY
+ SG_ BOH_3 : 43|1@0+ (1,0) [0|3] "" BDY
+ SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY
+ SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY
+ SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY
+ SG_ ZEROS_BOH : 7|24@0+ (0.002759506,0) [0|100] "m/s" BDY
+ SG_ FCM_OFF : 35|1@0+ (1,0) [0|1] "" BDY
+ SG_ SET_TO_1 : 36|1@0+ (1,0) [0|1] "" XXX
+ SG_ HUD_DISTANCE : 47|2@0+ (1,0) [0|3] "" BDY
+ SG_ HUD_LEAD : 45|2@0+ (1,0) [0|3] "" BDY
+ SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY
+ SG_ ACC_ON : 52|1@0+ (1,0) [0|1] "" XXX
+ SG_ BOH_6 : 51|4@0+ (1,0) [0|15] "" XXX
+ SG_ SET_TO_X3 : 55|2@0+ (1,0) [0|3] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 804 CRUISE: 8 PCM
+ SG_ TRIP_FUEL_CONSUMED : 23|16@0+ (1,0) [0|255] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 806 SCM_FEEDBACK: 8 SCM
+ SG_ DRIVERS_DOOR_OPEN : 17|1@0+ (1,0) [0|1] "" XXX
+ SG_ MAIN_ON : 28|1@0+ (1,0) [0|1] "" EON
+ SG_ RIGHT_BLINKER : 27|1@0+ (1,0) [0|1] "" EON
+ SG_ LEFT_BLINKER : 26|1@0+ (1,0) [0|1] "" EON
+ SG_ CMBS_STATES : 22|2@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 829 LKAS_HUD: 5 ADAS
+ SG_ CAM_TEMP_HIGH : 7|1@0+ (1,0) [0|255] "" BDY
+ SG_ SET_ME_X41 : 6|7@0+ (1,0) [0|127] "" BDY
+ SG_ BOH : 6|7@0+ (1,0) [0|127] "" BDY
+ SG_ DASHED_LANES : 14|1@0+ (1,0) [0|1] "" BDY
+ SG_ DTC : 13|1@0+ (1,0) [0|1] "" BDY
+ SG_ LKAS_PROBLEM : 12|1@0+ (1,0) [0|1] "" BDY
+ SG_ LKAS_OFF : 11|1@0+ (1,0) [0|1] "" BDY
+ SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY
+ SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY
+ SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY
+ SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY
+ SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY
+ SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY
+ SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY
+ SG_ LDW_OFF : 27|1@0+ (1,0) [0|1] "" BDY
+ SG_ CLEAN_WINDSHIELD : 26|1@0+ (1,0) [0|1] "" BDY
+ SG_ SET_ME_X48 : 31|8@0+ (1,0) [0|255] "" BDY
+ SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" BDY
+ SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" BDY
+
+BO_ 862 CAMERA_MESSAGES: 8 CAM
+ SG_ ZEROS_BOH : 7|50@0+ (1,0) [0|127] "" BDY
+ SG_ AUTO_HIGHBEAMS_ACTIVE : 53|1@0+ (1,0) [0|1] "" XXX
+ SG_ HIGHBEAMS_ON : 52|1@0+ (1,0) [0|1] "" XXX
+ SG_ ZEROS_BOH_2 : 51|4@0+ (1,0) [0|15] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 884 STALK_STATUS: 8 XXX
+ SG_ AUTO_HEADLIGHTS : 46|1@0+ (1,0) [0|1] "" EON
+ SG_ HIGH_BEAM_HOLD : 47|1@0+ (1,0) [0|1] "" EON
+ SG_ HIGH_BEAM_FLASH : 45|1@0+ (1,0) [0|1] "" EON
+ SG_ HEADLIGHTS_ON : 54|1@0+ (1,0) [0|1] "" EON
+ SG_ WIPER_SWITCH : 53|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+
+BO_ 891 STALK_STATUS_2: 8 XXX
+ SG_ WIPERS : 17|2@0+ (1,0) [0|3] "" EON
+ SG_ LOW_BEAMS : 35|1@0+ (1,0) [0|1] "" XXX
+ SG_ HIGH_BEAMS : 34|1@0+ (1,0) [0|1] "" XXX
+ SG_ PARK_LIGHTS : 36|1@0+ (1,0) [0|1] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+
+CM_ "honda_accord_lx15t_2018_can.dbc starts here"
+
+
+BO_ 401 GEARBOX: 8 PCM
+ SG_ GEAR_SHIFTER : 5|6@0+ (1,0) [0|63] "" EON
+ SG_ BOH : 45|6@0+ (1,0) [0|63] "" XXX
+ SG_ GEAR2 : 31|8@0+ (1,0) [0|1] "" XXX
+ SG_ GEAR : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ ZEROS_BOH : 47|2@0+ (1,0) [0|3] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
+
+BO_ 432 STANDSTILL: 7 VSA
+ SG_ BRAKE_ERROR_1 : 11|1@0+ (1,0) [0|1] "" EON
+ SG_ BRAKE_ERROR_2 : 9|1@0+ (1,0) [0|1] "" EON
+ SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON
+
+BO_ 446 BRAKE_MODULE: 3 VSA
+ SG_ BRAKE_PRESSED : 4|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 21|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 19|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 927 RADAR_HUD: 8 RADAR
+ SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY
+ SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY
+ SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX
+ SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY
+ SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX
+ SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY
+ SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX
+ SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ BOH : 40|1@0+ (1,0) [0|1] "" XXX
+ SG_ BOH_2 : 30|1@0+ (1,0) [0|1] "" XXX
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+
+BO_ 1302 ODOMETER: 8 XXX
+ SG_ ODOMETER : 7|24@0+ (1,0) [0|16777215] "km" EON
+ SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
+ SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
+
+VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P" ;
+VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ;
+VAL_ 545 ECON_ON_2 0 "off" 3 "on" ;
+VAL_ 662 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ;
+VAL_ 662 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
+VAL_ 806 CMBS_BUTTON 3 "pressed" 0 "released" ;
+VAL_ 891 WIPERS 4 "High" 2 "Low" 0 "Off" ;
+VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
+
+CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
diff --git a/opendbc/honda_accord_s2t_2018_can_generated.dbc b/opendbc/honda_accord_s2t_2018_can_generated.dbc
index 6d83a6b8c..663c1acec 100644
--- a/opendbc/honda_accord_s2t_2018_can_generated.dbc
+++ b/opendbc/honda_accord_s2t_2018_can_generated.dbc
@@ -68,10 +68,10 @@ BO_ 330 STEERING_SENSORS: 8 EPS
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
BO_ 344 ENGINE_DATA: 8 PCM
- SG_ XMISSION_SPEED : 7|16@0+ (0.002759506,0) [0|70] "m/s" EON
+ SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
- SG_ XMISSION_SPEED2 : 39|16@0+ (0.002759506,0) [0|70] "m/s" EON
- SG_ DISTANCE_COUNTER : 55|8@0+ (10,0) [0|2550] "Meters" XXX
+ SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
index bc4933387..42d4e7c96 100644
--- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
+++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
@@ -68,10 +68,10 @@ BO_ 330 STEERING_SENSORS: 8 EPS
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
BO_ 344 ENGINE_DATA: 8 PCM
- SG_ XMISSION_SPEED : 7|16@0+ (0.002759506,0) [0|70] "m/s" EON
+ SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
- SG_ XMISSION_SPEED2 : 39|16@0+ (0.002759506,0) [0|70] "m/s" EON
- SG_ DISTANCE_COUNTER : 55|8@0+ (10,0) [0|2550] "Meters" XXX
+ SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc
index 605d80d0d..0ec2f5846 100644
--- a/opendbc/honda_civic_touring_2016_can_generated.dbc
+++ b/opendbc/honda_civic_touring_2016_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc
index edcdf64d5..f59b155d2 100644
--- a/opendbc/honda_crv_ex_2017_can_generated.dbc
+++ b/opendbc/honda_crv_ex_2017_can_generated.dbc
@@ -68,10 +68,10 @@ BO_ 330 STEERING_SENSORS: 8 EPS
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
BO_ 344 ENGINE_DATA: 8 PCM
- SG_ XMISSION_SPEED : 7|16@0+ (0.002759506,0) [0|70] "m/s" EON
+ SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
- SG_ XMISSION_SPEED2 : 39|16@0+ (0.002759506,0) [0|70] "m/s" EON
- SG_ DISTANCE_COUNTER : 55|8@0+ (10,0) [0|2550] "Meters" XXX
+ SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
diff --git a/opendbc/honda_crv_touring_2016_can_generated.dbc b/opendbc/honda_crv_touring_2016_can_generated.dbc
index 57ae6ae5f..d41386efa 100644
--- a/opendbc/honda_crv_touring_2016_can_generated.dbc
+++ b/opendbc/honda_crv_touring_2016_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/honda_fit_ex_2018_can_generated.dbc b/opendbc/honda_fit_ex_2018_can_generated.dbc
index e2187497f..9276d3195 100644
--- a/opendbc/honda_fit_ex_2018_can_generated.dbc
+++ b/opendbc/honda_fit_ex_2018_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/honda_odyssey_exl_2018_generated.dbc b/opendbc/honda_odyssey_exl_2018_generated.dbc
index ddabe780d..e6ff81cc3 100644
--- a/opendbc/honda_odyssey_exl_2018_generated.dbc
+++ b/opendbc/honda_odyssey_exl_2018_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/honda_pilot_touring_2017_can_generated.dbc b/opendbc/honda_pilot_touring_2017_can_generated.dbc
index 51cd751c2..d8accf081 100644
--- a/opendbc/honda_pilot_touring_2017_can_generated.dbc
+++ b/opendbc/honda_pilot_touring_2017_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
index 40ff41e9c..73be13bc8 100644
--- a/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
+++ b/opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc
@@ -59,9 +59,9 @@ BO_ 344 ENGINE_DATA: 8 PCM
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
+ SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
- SG_ ODOMETER : 55|8@0+ (0.010588,0) [0|255] "km" XXX
BO_ 380 POWERTRAIN_DATA: 8 PCM
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml
index e372b2a30..38042afd0 100644
--- a/panda/.circleci/config.yml
+++ b/panda/.circleci/config.yml
@@ -25,9 +25,13 @@ jobs:
command: |
docker run panda_build /bin/bash -c "cd /panda; python setup.py install"
- run:
- name: Build STM image
+ name: Build Panda STM image
command: |
docker run panda_build /bin/bash -c "cd /panda/board; make bin"
+ - run:
+ name: Build NEO STM image
+ command: |
+ docker run panda_build /bin/bash -c "cd /panda/board; make clean; make -f Makefile.legacy obj/comma.bin"
- run:
name: Build ESP image
command: |
diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h
index cd809704b..943f268a9 100644
--- a/panda/board/drivers/can.h
+++ b/panda/board/drivers/can.h
@@ -461,8 +461,6 @@ void CAN3_SCE_IRQHandler() { can_sce(CAN3); }
#endif
-#include "canbitbang.h"
-
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
if (safety_tx_hook(to_push) && !can_autobaud_enabled[bus_number]) {
if (bus_number < BUS_MAX) {
diff --git a/panda/board/drivers/canbitbang.h b/panda/board/drivers/canbitbang.h
deleted file mode 100644
index 5226366b1..000000000
--- a/panda/board/drivers/canbitbang.h
+++ /dev/null
@@ -1,203 +0,0 @@
-#define MAX_BITS_CAN_PACKET (200)
-
-// returns out_len
-int do_bitstuff(char *out, char *in, int in_len) {
- int last_bit = -1;
- int bit_cnt = 0;
- int j = 0;
- for (int i = 0; i < in_len; i++) {
- char bit = in[i];
- out[j++] = bit;
-
- // do the stuffing
- if (bit == last_bit) {
- bit_cnt++;
- if (bit_cnt == 5) {
- // 5 in a row the same, do stuff
- last_bit = !bit;
- out[j++] = last_bit;
- bit_cnt = 1;
- }
- } else {
- // this is a new bit
- last_bit = bit;
- bit_cnt = 1;
- }
- }
- return j;
-}
-
-int append_crc(char *in, int in_len) {
- int crc = 0;
- for (int i = 0; i < in_len; i++) {
- crc <<= 1;
- if (in[i] ^ ((crc>>15)&1)) {
- crc = crc ^ 0x4599;
- }
- crc &= 0x7fff;
- }
- for (int i = 14; i >= 0; i--) {
- in[in_len++] = (crc>>i)&1;
- }
- return in_len;
-}
-
-int append_bits(char *in, int in_len, char *app, int app_len) {
- for (int i = 0; i < app_len; i++) {
- in[in_len++] = app[i];
- }
- return in_len;
-}
-
-int append_int(char *in, int in_len, int val, int val_len) {
- for (int i = val_len-1; i >= 0; i--) {
- in[in_len++] = (val&(1<RDTR & 0xF;
- len = append_int(pkt, len, 0, 1); // Start-of-frame
-
- if (to_bang->RIR & 4) {
- // extended identifier
- len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
- len = append_int(pkt, len, 3, 2); // SRR+IDE
- len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1<<18)-1), 18); // Identifier
- len = append_int(pkt, len, 0, 3); // RTR+r1+r0
- } else {
- // standard identifier
- len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
- len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved
- }
-
- len = append_int(pkt, len, dlc_len, 4); // Data length code
-
- // append data
- for (int i = 0; i < dlc_len; i++) {
- unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
- len = append_int(pkt, len, dat, 8);
- }
-
- // append crc
- len = append_crc(pkt, len);
-
- // do bitstuffing
- len = do_bitstuff(out, pkt, len);
-
- // append footer
- len = append_bits(out, len, footer, sizeof(footer));
- return len;
-}
-
-// hardware stuff below this line
-
-#ifdef PANDA
-
-void set_bitbanged_gmlan(int val) {
- if (val) {
- GPIOB->ODR |= (1 << 13);
- } else {
- GPIOB->ODR &= ~(1 << 13);
- }
-}
-
-char pkt_stuffed[MAX_BITS_CAN_PACKET];
-int gmlan_sending = -1;
-int gmlan_sendmax = -1;
-
-int gmlan_silent_count = 0;
-int gmlan_fail_count = 0;
-#define REQUIRED_SILENT_TIME 10
-#define MAX_FAIL_COUNT 10
-
-void TIM4_IRQHandler(void) {
- if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) {
- int read = get_gpio_input(GPIOB, 12);
- if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
- if (read == 0) {
- gmlan_silent_count = 0;
- } else {
- gmlan_silent_count++;
- }
- } else if (gmlan_silent_count == REQUIRED_SILENT_TIME) {
- int retry = 0;
- // in send loop
- if (gmlan_sending > 0 && // not first bit
- (read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant
- gmlan_sending != (gmlan_sendmax-11)) { //not ack bit
- puts("GMLAN ERR: bus driven at ");
- puth(gmlan_sending);
- puts("\n");
- retry = 1;
- } else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK
- puts("GMLAN ERR: didn't recv ACK\n");
- retry = 1;
- }
- if (retry) {
- // reset sender (retry after 7 silent)
- set_bitbanged_gmlan(1); // recessive
- gmlan_silent_count = 0;
- gmlan_sending = 0;
- gmlan_fail_count++;
- if (gmlan_fail_count == MAX_FAIL_COUNT) {
- puts("GMLAN ERR: giving up send\n");
- }
- } else {
- set_bitbanged_gmlan(pkt_stuffed[gmlan_sending]);
- gmlan_sending++;
- }
- }
- if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) {
- set_bitbanged_gmlan(1); // recessive
- set_gpio_mode(GPIOB, 13, MODE_INPUT);
- TIM4->DIER = 0; // no update interrupt
- TIM4->CR1 = 0; // disable timer
- gmlan_sendmax = -1; // exit
- }
- }
- TIM4->SR = 0;
-}
-
-void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
- // TODO: make failure less silent
- if (gmlan_sendmax != -1) return;
-
- int len = get_bit_message(pkt_stuffed, to_bang);
- gmlan_fail_count = 0;
- gmlan_silent_count = 0;
- gmlan_sending = 0;
- gmlan_sendmax = len;
-
- // setup for bitbang loop
- set_bitbanged_gmlan(1); // recessive
- set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
-
- // setup
- TIM4->PSC = 48-1; // tick on 1 us
- TIM4->CR1 = TIM_CR1_CEN; // enable
- TIM4->ARR = 30-1; // 33.3 kbps
-
- // in case it's disabled
- NVIC_EnableIRQ(TIM4_IRQn);
-
- // run the interrupt
- TIM4->DIER = TIM_DIER_UIE; // update interrupt
- TIM4->SR = 0;
-}
-
-#endif
-
diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h
new file mode 100644
index 000000000..8521100a8
--- /dev/null
+++ b/panda/board/drivers/gmlan_alt.h
@@ -0,0 +1,276 @@
+#define GMLAN_TICKS_PER_SECOND 33300 //1sec @ 33.3kbps
+#define GMLAN_TICKS_PER_TIMEOUT_TICKLE 500 //15ms @ 33.3kbps
+#define GMLAN_HIGH 0 //0 is high on bus (dominant)
+#define GMLAN_LOW 1 //1 is low on bus
+
+#define DISABLED -1
+#define BITBANG 0
+#define GPIO_SWITCH 1
+
+#define MAX_BITS_CAN_PACKET (200)
+
+int gmlan_alt_mode = DISABLED;
+
+// returns out_len
+int do_bitstuff(char *out, char *in, int in_len) {
+ int last_bit = -1;
+ int bit_cnt = 0;
+ int j = 0;
+ for (int i = 0; i < in_len; i++) {
+ char bit = in[i];
+ out[j++] = bit;
+
+ // do the stuffing
+ if (bit == last_bit) {
+ bit_cnt++;
+ if (bit_cnt == 5) {
+ // 5 in a row the same, do stuff
+ last_bit = !bit;
+ out[j++] = last_bit;
+ bit_cnt = 1;
+ }
+ } else {
+ // this is a new bit
+ last_bit = bit;
+ bit_cnt = 1;
+ }
+ }
+ return j;
+}
+
+int append_crc(char *in, int in_len) {
+ int crc = 0;
+ for (int i = 0; i < in_len; i++) {
+ crc <<= 1;
+ if (in[i] ^ ((crc>>15)&1)) {
+ crc = crc ^ 0x4599;
+ }
+ crc &= 0x7fff;
+ }
+ for (int i = 14; i >= 0; i--) {
+ in[in_len++] = (crc>>i)&1;
+ }
+ return in_len;
+}
+
+int append_bits(char *in, int in_len, char *app, int app_len) {
+ for (int i = 0; i < app_len; i++) {
+ in[in_len++] = app[i];
+ }
+ return in_len;
+}
+
+int append_int(char *in, int in_len, int val, int val_len) {
+ for (int i = val_len-1; i >= 0; i--) {
+ in[in_len++] = (val&(1<RDTR & 0xF;
+ len = append_int(pkt, len, 0, 1); // Start-of-frame
+
+ if (to_bang->RIR & 4) {
+ // extended identifier
+ len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
+ len = append_int(pkt, len, 3, 2); // SRR+IDE
+ len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1<<18)-1), 18); // Identifier
+ len = append_int(pkt, len, 0, 3); // RTR+r1+r0
+ } else {
+ // standard identifier
+ len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
+ len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved
+ }
+
+ len = append_int(pkt, len, dlc_len, 4); // Data length code
+
+ // append data
+ for (int i = 0; i < dlc_len; i++) {
+ unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
+ len = append_int(pkt, len, dat, 8);
+ }
+
+ // append crc
+ len = append_crc(pkt, len);
+
+ // do bitstuffing
+ len = do_bitstuff(out, pkt, len);
+
+ // append footer
+ len = append_bits(out, len, footer, sizeof(footer));
+ return len;
+}
+
+#ifdef PANDA
+
+void setup_timer4() {
+ // setup
+ TIM4->PSC = 48-1; // tick on 1 us
+ TIM4->CR1 = TIM_CR1_CEN; // enable
+ TIM4->ARR = 30-1; // 33.3 kbps
+
+ // in case it's disabled
+ NVIC_EnableIRQ(TIM4_IRQn);
+
+ // run the interrupt
+ TIM4->DIER = TIM_DIER_UIE; // update interrupt
+ TIM4->SR = 0;
+}
+
+int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms
+int can_timeout_counter = GMLAN_TICKS_PER_SECOND; //1 second
+
+int inverted_bit_to_send = GMLAN_HIGH;
+int gmlan_switch_below_timeout = -1;
+int gmlan_switch_timeout_enable = 0;
+
+void gmlan_switch_init(int timeout_enable) {
+ gmlan_switch_timeout_enable = timeout_enable;
+ gmlan_alt_mode = GPIO_SWITCH;
+ gmlan_switch_below_timeout = 1;
+ set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
+
+ setup_timer4();
+
+ inverted_bit_to_send = GMLAN_LOW; //We got initialized, set the output low
+}
+
+void set_gmlan_digital_output(int to_set) {
+ inverted_bit_to_send = to_set;
+ /*
+ puts("Writing ");
+ puth(inverted_bit_to_send);
+ puts("\n");
+ */
+}
+
+void reset_gmlan_switch_timeout(void) {
+ can_timeout_counter = GMLAN_TICKS_PER_SECOND;
+ gmlan_switch_below_timeout = 1;
+ gmlan_alt_mode = GPIO_SWITCH;
+}
+
+void set_bitbanged_gmlan(int val) {
+ if (val) {
+ GPIOB->ODR |= (1 << 13);
+ } else {
+ GPIOB->ODR &= ~(1 << 13);
+ }
+}
+
+char pkt_stuffed[MAX_BITS_CAN_PACKET];
+int gmlan_sending = -1;
+int gmlan_sendmax = -1;
+
+int gmlan_silent_count = 0;
+int gmlan_fail_count = 0;
+#define REQUIRED_SILENT_TIME 10
+#define MAX_FAIL_COUNT 10
+
+void TIM4_IRQHandler(void) {
+ if (gmlan_alt_mode == BITBANG) {
+ if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) {
+ int read = get_gpio_input(GPIOB, 12);
+ if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
+ if (read == 0) {
+ gmlan_silent_count = 0;
+ } else {
+ gmlan_silent_count++;
+ }
+ } else if (gmlan_silent_count == REQUIRED_SILENT_TIME) {
+ int retry = 0;
+ // in send loop
+ if (gmlan_sending > 0 && // not first bit
+ (read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant
+ gmlan_sending != (gmlan_sendmax-11)) { //not ack bit
+ puts("GMLAN ERR: bus driven at ");
+ puth(gmlan_sending);
+ puts("\n");
+ retry = 1;
+ } else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK
+ puts("GMLAN ERR: didn't recv ACK\n");
+ retry = 1;
+ }
+ if (retry) {
+ // reset sender (retry after 7 silent)
+ set_bitbanged_gmlan(1); // recessive
+ gmlan_silent_count = 0;
+ gmlan_sending = 0;
+ gmlan_fail_count++;
+ if (gmlan_fail_count == MAX_FAIL_COUNT) {
+ puts("GMLAN ERR: giving up send\n");
+ }
+ } else {
+ set_bitbanged_gmlan(pkt_stuffed[gmlan_sending]);
+ gmlan_sending++;
+ }
+ }
+ if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) {
+ set_bitbanged_gmlan(1); // recessive
+ set_gpio_mode(GPIOB, 13, MODE_INPUT);
+ TIM4->DIER = 0; // no update interrupt
+ TIM4->CR1 = 0; // disable timer
+ gmlan_sendmax = -1; // exit
+ }
+ }
+ TIM4->SR = 0;
+ } //bit bang mode
+
+ else if (gmlan_alt_mode == GPIO_SWITCH) {
+ if (TIM4->SR & TIM_SR_UIF && gmlan_switch_below_timeout != -1) {
+ if (can_timeout_counter == 0 && gmlan_switch_timeout_enable) {
+ //it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output
+ set_gpio_output(GPIOB, 13, GMLAN_LOW);
+ gmlan_switch_below_timeout = -1;
+ gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE;
+ gmlan_alt_mode = DISABLED;
+ }
+ else {
+ can_timeout_counter--;
+ if (gmlan_timeout_counter == 0) {
+ //Send a 1 (bus low) every 15ms to reset the GMLAN transceivers timeout
+ gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE;
+ set_gpio_output(GPIOB, 13, GMLAN_LOW);
+ }
+ else {
+ set_gpio_output(GPIOB, 13, inverted_bit_to_send);
+ gmlan_timeout_counter--;
+ }
+ }
+ }
+ TIM4->SR = 0;
+ } //gmlan switch mode
+}
+
+void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
+ gmlan_alt_mode = BITBANG;
+ // TODO: make failure less silent
+ if (gmlan_sendmax != -1) return;
+
+ int len = get_bit_message(pkt_stuffed, to_bang);
+ gmlan_fail_count = 0;
+ gmlan_silent_count = 0;
+ gmlan_sending = 0;
+ gmlan_sendmax = len;
+
+ // setup for bitbang loop
+ set_bitbanged_gmlan(1); // recessive
+ set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
+
+ setup_timer4();
+}
+
+#endif
diff --git a/panda/board/main.c b/panda/board/main.c
index a15861a17..8bb6a0cee 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -15,6 +15,7 @@
#include "drivers/uart.h"
#include "drivers/adc.h"
#include "drivers/usb.h"
+#include "drivers/gmlan_alt.h"
#include "drivers/can.h"
#include "drivers/spi.h"
#include "drivers/timer.h"
diff --git a/panda/board/safety.h b/panda/board/safety.h
index 4b5a84084..4c24bd9b2 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -5,6 +5,14 @@ struct sample_t {
int max;
} sample_t_default = {{0}, 0, 0};
+// no float support in STM32F2 micros (cortex-m3)
+#ifdef PANDA
+struct lookup_t {
+ float x[3];
+ float y[3];
+};
+#endif
+
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
@@ -16,9 +24,12 @@ int max_limit_check(int val, const int MAX, const int MIN);
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
int driver_limit_check(int val, int val_last, struct sample_t *val_driver,
- const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
+ const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
+#ifdef PANDA
+float interpolate(struct lookup_t xy, float x);
+#endif
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@@ -49,6 +60,7 @@ int controls_allowed = 0;
#include "safety/safety_gm.h"
#include "safety/safety_ford.h"
#include "safety/safety_cadillac.h"
+#include "safety/safety_hyundai.h"
#include "safety/safety_elm327.h"
const safety_hooks *current_hooks = &nooutput_hooks;
@@ -87,6 +99,7 @@ typedef struct {
#define SAFETY_HONDA_BOSCH 4
#define SAFETY_FORD 5
#define SAFETY_CADILLAC 6
+#define SAFETY_HYUNDAI 7
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@@ -100,6 +113,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_GM, &gm_hooks},
{SAFETY_FORD, &ford_hooks},
{SAFETY_CADILLAC, &cadillac_hooks},
+ {SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
#ifdef PANDA
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
@@ -201,3 +215,31 @@ int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// check for violation
return (val < lowest_val) || (val > highest_val);
}
+
+
+#ifdef PANDA
+// interp function that holds extreme values
+float interpolate(struct lookup_t xy, float x) {
+ int size = sizeof(xy.x) / sizeof(xy.x[0]);
+ // x is lower than the first point in the x array. Return the first point
+ if (x <= xy.x[0]) {
+ return xy.y[0];
+
+ } else {
+ // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
+ for (int i=0; i < size-1; i++) {
+ if (x < xy.x[i+1]) {
+ float x0 = xy.x[i];
+ float y0 = xy.y[i];
+ float dx = xy.x[i+1] - x0;
+ float dy = xy.y[i+1] - y0;
+ // dx should not be zero as xy.x is supposed ot be monotonic
+ if (dx <= 0.) dx = 0.0001;
+ return dy * (x - x0) / dx + y0;
+ }
+ }
+ // if no such point is found, then x > xy.x[size-1]. Return last point
+ return xy.y[size - 1];
+ }
+}
+#endif
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
new file mode 100644
index 000000000..e23dc7372
--- /dev/null
+++ b/panda/board/safety/safety_hyundai.h
@@ -0,0 +1,36 @@
+int hyundai_giraffe_switch_1 = 0; // is giraffe switch 1 high?
+
+
+static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+
+ int bus = (to_push->RDTR >> 4) & 0xF;
+ // 832 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high and we want stock
+ if ((to_push->RIR>>21) == 832 && (bus == 0)) {
+ hyundai_giraffe_switch_1 = 1;
+ }
+}
+
+static void hyundai_init(int16_t param) {
+ controls_allowed = 0;
+ hyundai_giraffe_switch_1 = 0;
+}
+
+static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
+
+ // forward camera to car and viceversa, excpet for lkas11 and mdps12
+ if ((bus_num == 0 || bus_num == 2) && !hyundai_giraffe_switch_1) {
+ int addr = to_fwd->RIR>>21;
+ bool is_lkas_msg = (addr == 832 && bus_num == 2) || (addr == 593 && bus_num == 0);
+ return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2);
+ }
+ return -1;
+}
+
+const safety_hooks hyundai_hooks = {
+ .init = hyundai_init,
+ .rx = hyundai_rx_hook,
+ .tx = nooutput_tx_hook,
+ .tx_lin = nooutput_tx_lin_hook,
+ .ignition = default_ign_hook,
+ .fwd = hyundai_fwd_hook,
+};
diff --git a/panda/board/safety/safety_toyota_ipas.h b/panda/board/safety/safety_toyota_ipas.h
index dc265fa03..1627ece3a 100644
--- a/panda/board/safety/safety_toyota_ipas.h
+++ b/panda/board/safety/safety_toyota_ipas.h
@@ -4,11 +4,6 @@
// IPAS override
const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
-struct lookup_t {
- float x[3];
- float y[3];
-};
-
// 2m/s are added to be less restrictive
const struct lookup_t LOOKUP_ANGLE_RATE_UP = {
{2., 7., 17.},
@@ -35,31 +30,6 @@ uint32_t ts_angle_last = 0;
int controls_allowed_last = 0;
-// interp function that holds extreme values
-float interpolate(struct lookup_t xy, float x) {
- int size = sizeof(xy.x) / sizeof(xy.x[0]);
- // x is lower than the first point in the x array. Return the first point
- if (x <= xy.x[0]) {
- return xy.y[0];
-
- } else {
- // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
- for (int i=0; i < size-1; i++) {
- if (x < xy.x[i+1]) {
- float x0 = xy.x[i];
- float y0 = xy.y[i];
- float dx = xy.x[i+1] - x0;
- float dy = xy.y[i+1] - y0;
- // dx should not be zero as xy.x is supposed ot be monotonic
- if (dx <= 0.) dx = 0.0001;
- return dy * (x - x0) / dx + y0;
- }
- }
- // if no such point is found, then x > xy.x[size-1]. Return last point
- return xy.y[size - 1];
- }
-}
-
static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// check standard toyota stuff as well
@@ -146,15 +116,15 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int delta_angle_down = (int) (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG + 1.);
int highest_desired_angle = desired_angle_last + (desired_angle_last > 0? delta_angle_up:delta_angle_down);
int lowest_desired_angle = desired_angle_last - (desired_angle_last > 0? delta_angle_down:delta_angle_up);
- if ((desired_angle > highest_desired_angle) ||
+ if ((desired_angle > highest_desired_angle) ||
(desired_angle < lowest_desired_angle)){
violation = 1;
controls_allowed = 0;
}
}
-
+
// desired steer angle should be the same as steer angle measured when controls are off
- if ((!controls_allowed) &&
+ if ((!controls_allowed) &&
((desired_angle < (angle_meas.min - 1)) ||
(desired_angle > (angle_meas.max + 1)) ||
(ipas_state_cmd != 1))) {
diff --git a/panda/boardesp/README.md b/panda/boardesp/README.md
index 9b15ef235..5a8fab147 100644
--- a/panda/boardesp/README.md
+++ b/panda/boardesp/README.md
@@ -2,13 +2,13 @@
Dependencies
-----
-**Mac**
+**Debian / Ubuntu**
```
./get_sdk.sh
```
-**Debian / Ubuntu**
+**Mac**
```
./get_sdk_mac.sh
diff --git a/panda/examples/get_panda_password.py b/panda/examples/get_panda_password.py
new file mode 100644
index 000000000..11071d035
--- /dev/null
+++ b/panda/examples/get_panda_password.py
@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+from panda import Panda
+
+def get_panda_password():
+
+ try:
+ print("Trying to connect to Panda over USB...")
+ p = Panda()
+
+ except AssertionError:
+ print("USB connection failed")
+ sys.exit(0)
+
+ wifi = p.get_serial()
+ #print('[%s]' % ', '.join(map(str, wifi)))
+ print("SSID: " + wifi[0])
+ print("Password: " + wifi[1])
+
+if __name__ == "__main__":
+ get_panda_password()
\ No newline at end of file
diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt
index 03459175d..deb37d086 100644
--- 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
diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h
index accb593ff..f8d07ae68 100644
--- 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);
diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc
index 3615e0529..2d4fb9570 100644
--- 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}})
diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py
index 3c5d440f3..09f5e5121 100644
--- 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;
diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc
index 2443f10f7..b1fd8e5d5 100644
--- a/selfdrive/can/packer.cc
+++ b/selfdrive/can/packer.cc
@@ -13,9 +13,26 @@
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);
+ mask = ReverseBytes(mask);
+ }
ret &= ~mask;
ret |= dat;
return ret;
diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py
index 58f5e3efc..895538393 100644
--- 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"))
diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py
index 6abbb3c18..165673cea 100644
--- 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
diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py
index b1e66780a..331e0a313 100755
--- 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)
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index 4626d28f4..c860cfdee 100644
--- 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 = [
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 3d0a49fee..bf39272fc 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -194,7 +194,8 @@ class CarInterface(object):
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
- ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
+ cruiseEnabled = self.CS.pcm_acc_status != 0
+ ret.cruiseState.enabled = cruiseEnabled
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
ret.leftBlinker = self.CS.left_blinker_on
@@ -228,7 +229,8 @@ class CarInterface(object):
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
- be.type = 'accelCruise'
+ if not (cruiseEnabled and self.CS.standstill):
+ be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
elif but == CruiseButtons.CANCEL:
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 466259c20..dd575c3c6 100644
--- 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.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):
@@ -119,7 +88,7 @@ def get_can_signals(CP):
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
checks += [("CRUISE_PARAMS", 50)]
- if CP.carFingerprint == CAR.ACCORD:
+ if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15):
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
else:
signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1),
@@ -137,9 +106,7 @@ def get_can_signals(CP):
elif CP.carFingerprint == CAR.ACURA_ILX:
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_BUTTONS", 0)]
- elif CP.carFingerprint == CAR.CRV:
- signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
- elif CP.carFingerprint == CAR.ACURA_RDX:
+ elif CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE):
signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
elif CP.carFingerprint == CAR.ODYSSEY:
signals += [("MAIN_ON", "SCM_FEEDBACK", 0),
@@ -149,8 +116,6 @@ def get_can_signals(CP):
elif CP.carFingerprint == CAR.PILOT:
signals += [("MAIN_ON", "SCM_BUTTONS", 0),
("CAR_GAS", "GAS_PEDAL_2", 0)]
- elif CP.carFingerprint == CAR.RIDGELINE:
- signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
@@ -168,6 +133,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
@@ -212,7 +179,7 @@ class CarState(object):
# ******************* parse out can *******************
- if self.CP.carFingerprint == CAR.ACCORD: # TODO: find wheels moving bit in dbc
+ if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15): # TODO: find wheels moving bit in dbc
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
else:
@@ -230,15 +197,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 +223,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']
@@ -266,7 +234,7 @@ class CarState(object):
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
- if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_HATCH):
+ if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.CIVIC_HATCH):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
@@ -275,11 +243,12 @@ 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
- if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE):
+ if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019):
self.car_gas = self.pedal_gas
else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 8dc5e1d9b..a48c199d9 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -2,7 +2,7 @@ import struct
import common.numpy_fast as np
from selfdrive.config import Conversions as CV
-from selfdrive.car.honda.values import CAR
+from selfdrive.car.honda.values import CAR, HONDA_BOSCH
# *** Honda specific ***
def can_cksum(mm):
@@ -70,7 +70,7 @@ def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, i
"STEER_TORQUE_REQUEST": lkas_active,
}
# Set bus 2 for accord and new crv.
- bus = 2 if car_fingerprint in (CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_HATCH) else 0
+ bus = 2 if car_fingerprint in HONDA_BOSCH else 0
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
@@ -80,7 +80,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
bus = 0
# Bosch sends commands to bus 2.
- if car_fingerprint in (CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_HATCH):
+ if car_fingerprint in HONDA_BOSCH:
bus = 2
else:
acc_hud_values = {
@@ -142,6 +142,8 @@ def create_radar_commands(v_ego, car_fingerprint, new_radar_config, idx):
msg_0x301 = "\x0f\x18\x51\x02\x5a\x00\x00"
elif car_fingerprint == CAR.PILOT:
msg_0x301 = "\x00\x00\x56\x02\x58\x00\x00"
+ elif car_fingerprint == CAR.PILOT_2019:
+ msg_0x301 = "\x00\x00\x58\x02\x5c\x00\x00"
elif car_fingerprint == CAR.RIDGELINE:
msg_0x301 = "\x00\x00\x56\x02\x57\x00\x00"
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 4cac8c092..91fbb078a 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -204,7 +204,7 @@ class CarInterface(object):
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
- elif candidate == CAR.ACCORD:
+ elif candidate in (CAR.ACCORD, CAR.ACCORD_15):
stop_and_go = True
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
ret.mass = 3279. * CV.LB_TO_KG + std_cargo
@@ -288,7 +288,7 @@ class CarInterface(object):
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
- elif candidate == CAR.PILOT:
+ elif candidate in (CAR.PILOT, CAR.PILOT_2019):
stop_and_go = False
ret.mass = 4303 * CV.LB_TO_KG + std_cargo
ret.wheelbase = 2.81
@@ -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]))
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 5188ca576..243849431 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -38,6 +38,7 @@ class AH:
class CAR:
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
+ ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T"
CIVIC = "HONDA CIVIC 2016 TOURING"
CIVIC_HATCH = "HONDA CIVIC HATCHBACK 2017 EX"
ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS"
@@ -46,6 +47,7 @@ class CAR:
ODYSSEY = "HONDA ODYSSEY 2018 EX-L"
ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS"
PILOT = "HONDA PILOT 2017 TOURING"
+ PILOT_2019 = "HONDA PILOT 2019 ELITE"
RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION"
@@ -53,13 +55,16 @@ FINGERPRINTS = {
CAR.ACCORD: [{
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
}],
+ CAR.ACCORD_15: [{
+ 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
+ }],
CAR.ACURA_ILX: [{
57: 3, 145: 8, 228: 5, 304: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 419: 8, 420: 8, 422: 8, 428: 8, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1030: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5,
}],
# Acura RDX w/ Added Comma Pedal Support (512L & 513L)
CAR.ACURA_RDX: [{
57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1
- }],
+ }],
CAR.CIVIC: [{
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 450: 8, 464: 8, 470: 2, 476: 7, 487: 4, 490: 8, 493: 5, 506: 8, 512: 6, 513: 6, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1633: 8,
}],
@@ -84,6 +89,9 @@ FINGERPRINTS = {
CAR.PILOT: [{
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5
}],
+ CAR.PILOT_2019: [{
+ 57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
+ }],
# Ridgeline w/ Added Comma Pedal Support (512L & 513L)
CAR.RIDGELINE: [{
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 471: 3, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1613: 5, 1616: 5, 1618: 5, 1668: 5, 2015: 3
@@ -93,6 +101,7 @@ FINGERPRINTS = {
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
+ CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
@@ -101,12 +110,14 @@ DBC = {
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None),
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
+ CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
}
STEER_THRESHOLD = {
CAR.ACCORD: 1200,
+ CAR.ACCORD_15: 1200,
CAR.ACURA_ILX: 1200,
CAR.ACURA_RDX: 400,
CAR.CIVIC: 1200,
@@ -115,8 +126,24 @@ STEER_THRESHOLD = {
CAR.CRV_5G: 1200,
CAR.ODYSSEY: 1200,
CAR.PILOT: 1200,
+ CAR.PILOT_2019: 1200,
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.PILOT_2019: 1.,
+ CAR.RIDGELINE: 1.,
+}
+
# TODO: get these from dbc file
-HONDA_BOSCH = [CAR.ACCORD, CAR.CIVIC_HATCH, CAR.CRV_5G]
+HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.CIVIC_HATCH, CAR.CRV_5G]
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 53ff3aaa8..7e518a57d 100644
--- 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
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 664167ebc..702230838 100644
--- 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"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index f75f4c70a..ed2ae032f 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -448,12 +448,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
fcw_enabled = params.get("IsFcwEnabled") == "1"
geofence = None
- try:
- from selfdrive.controls.lib.geofence import Geofence
- geofence = Geofence(params.get("IsGeofenceEnabled") == "1")
- except ImportError:
- # geofence not available
- params.put("IsGeofenceEnabled", "-1")
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb)
@@ -511,7 +505,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")
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index 2fd716c3d..f1c248491 100644
--- 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(
diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py
index a70eb4984..23e247ed7 100644
--- 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)
-
diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py
index df3bb0a96..834f65e50 100755
--- 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)
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
new file mode 100755
index 000000000..fcb78e5fe
--- /dev/null
+++ 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()
+
diff --git a/selfdrive/locationd/get_vp.c b/selfdrive/locationd/get_vp.c
new file mode 100644
index 000000000..c9ff3d604
--- /dev/null
+++ 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);
+}
diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd
index 67d21aad9..cb58f18aa 100755
Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 81a4bd981..26186f74f 100755
--- 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")
diff --git a/selfdrive/orbd/.gitignore b/selfdrive/orbd/.gitignore
new file mode 100644
index 000000000..829780eb5
--- /dev/null
+++ 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
diff --git a/selfdrive/orbd/Makefile b/selfdrive/orbd/Makefile
new file mode 100644
index 000000000..c33a8636b
--- /dev/null
+++ 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
+
diff --git a/selfdrive/orbd/dsp/freethedsp.c b/selfdrive/orbd/dsp/freethedsp.c
new file mode 100644
index 000000000..298f4fd83
--- /dev/null
+++ 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 ./'
+// 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
+#include
+#include
+#include
+#include
+
+// 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
+#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;
+}
+
diff --git a/selfdrive/orbd/dsp/gen/calculator.h b/selfdrive/orbd/dsp/gen/calculator.h
new file mode 100644
index 000000000..86a3de671
--- /dev/null
+++ b/selfdrive/orbd/dsp/gen/calculator.h
@@ -0,0 +1,39 @@
+#ifndef _CALCULATOR_H
+#define _CALCULATOR_H
+
+#include
+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
diff --git a/selfdrive/orbd/dsp/gen/calculator_stub.c b/selfdrive/orbd/dsp/gen/calculator_stub.c
new file mode 100644
index 000000000..66e4a0f82
--- /dev/null
+++ 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
+#include
+
+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
+ #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
+#include
+
+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
+
+//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
+
+#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
diff --git a/selfdrive/orbd/dsp/gen/libcalculator_skel.so b/selfdrive/orbd/dsp/gen/libcalculator_skel.so
new file mode 100755
index 000000000..e48cab482
Binary files /dev/null and b/selfdrive/orbd/dsp/gen/libcalculator_skel.so differ
diff --git a/selfdrive/orbd/extractor.h b/selfdrive/orbd/extractor.h
new file mode 100644
index 000000000..f506cd386
--- /dev/null
+++ b/selfdrive/orbd/extractor.h
@@ -0,0 +1,38 @@
+#ifndef EXTRACTOR_H
+#define EXTRACTOR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include
+
+#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
diff --git a/selfdrive/orbd/orbd.cc b/selfdrive/orbd/orbd.cc
new file mode 100644
index 000000000..ce0d47aec
--- /dev/null
+++ b/selfdrive/orbd/orbd.cc
@@ -0,0 +1,191 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "common/visionipc.h"
+#include "common/swaglog.h"
+
+#include "extractor.h"
+
+#ifdef DSP
+#include "dsp/gen/calculator.h"
+#else
+#include "turbocv.h"
+#endif
+
+#include
+#include
+#include "cereal/gen/cpp/log.capnp.h"
+
+#ifndef PATH_MAX
+#include
+#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();
+ 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 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();
+ 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;
+}
+
diff --git a/selfdrive/orbd/orbd_wrapper.sh b/selfdrive/orbd/orbd_wrapper.sh
new file mode 100755
index 000000000..8ec7443a3
--- /dev/null
+++ 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
+
diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd
index 84705bbc8..0fa7dac68 100755
Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ
diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord
index 49d7db520..42d0228e2 100755
Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index bc53a6a41..464afec30 100644
--- 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);
diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond
index 89ad0fcdf..5f34afbcc 100755
Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ