diff --git a/.gitignore b/.gitignore index 71ca33c652..70ad45c320 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 2357509eee..a65360ebe7 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 c8762963c0..d7f147c581 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 0a81ac30ed..1f822b6f67 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 65ec3ee957..c14ed5b997 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 fbb00a2a19..06392b88ae 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 1030309491..7135b07cf1 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 0000000000..264dbd7fcb --- /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 9d1b096af1..827cc21769 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, diff --git a/common/transformations/camera.py b/common/transformations/camera.py new file mode 100644 index 0000000000..e7472f5330 --- /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 0000000000..1a8122d6e3 --- /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 0000000000..9e4edd456b --- /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/requirements_openpilot.txt b/requirements_openpilot.txt index 03459175d4..deb37d0865 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 accb593ff3..f8d07ae681 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 3615e05297..2d4fb9570a 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 3c5d440f3b..09f5e5121d 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 2443f10f71..c79ce6cf64 100644 --- a/selfdrive/can/packer.cc +++ b/selfdrive/can/packer.cc @@ -13,9 +13,25 @@ namespace { + // this is the same as read_u64_le, but uses uint64_t as in/out + uint64_t ReverseBytes(uint64_t x) { + return ((x & 0xff00000000000000ull) >> 56) | + ((x & 0x00ff000000000000ull) >> 40) | + ((x & 0x0000ff0000000000ull) >> 24) | + ((x & 0x000000ff00000000ull) >> 8) | + ((x & 0x00000000ff000000ull) << 8) | + ((x & 0x0000000000ff0000ull) << 24) | + ((x & 0x000000000000ff00ull) << 40) | + ((x & 0x00000000000000ffull) << 56); + } + uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){ - uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo; - uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo; + int shift = sig.is_little_endian? sig.b1 : sig.bo; + uint64_t mask = ((1ULL << sig.b2)-1) << shift; + uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift; + if (sig.is_little_endian) { + dat = ReverseBytes(dat); + } ret &= ~mask; ret |= dat; return ret; diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index 58f5e3efc3..8955383934 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 6abbb3c18a..165673cea5 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 b1e66780a7..331e0a3130 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 4626d28f4a..c860cfdee4 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/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6db5df058e..90488cc86d 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 - -def parse_gear_shifter(can_gear_shifter, car_fingerprint): - - # TODO: Use VAL from DBC to parse this field - if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY): - if can_gear_shifter == 0x1: - return "park" - elif can_gear_shifter == 0x2: - return "reverse" - elif can_gear_shifter == 0x3: - return "neutral" - elif can_gear_shifter == 0x4: - return "drive" - elif can_gear_shifter == 0xa: - return "sport" - elif car_fingerprint in (CAR.CIVIC, CAR.CRV, CAR.ACURA_RDX, CAR.ACCORD_15, CAR.CRV_5G, CAR.CIVIC_HATCH): - if can_gear_shifter == 0x1: - return "park" - elif can_gear_shifter == 0x2: - return "reverse" - elif can_gear_shifter == 0x4: - return "neutral" - elif can_gear_shifter == 0x8: - return "drive" - elif can_gear_shifter == 0x10: - return "sport" - elif can_gear_shifter == 0x20: - return "low" - - elif car_fingerprint in (CAR.ACCORD, CAR.PILOT, CAR.RIDGELINE): - if can_gear_shifter == 0x8: - return "reverse" - elif can_gear_shifter == 0x4: - return "park" - elif can_gear_shifter == 0x20: - return "drive" - elif can_gear_shifter == 0x2: - return "sport" - - return "unknown" +from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR + +def parse_gear_shifter(gear, vals): + + val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral', + 'D': 'drive', 'S': 'sport', 'L': 'low'} + try: + return val_to_capnp[vals[gear]] + except KeyError: + return "unknown" def calc_cruise_offset(offset, speed): @@ -168,6 +137,8 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): self.CP = CP + self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 @@ -230,15 +201,17 @@ class CarState(object): self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] # calc best v_ego estimate, by averaging two opposite corners - self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS + speed_factor = SPEED_FACTOR[self.CP.carFingerprint] + self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor + self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor + self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor + self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor self.v_wheel = (self.v_wheel_fl+self.v_wheel_fr+self.v_wheel_rl+self.v_wheel_rr)/4. # blend in transmission speed at low speed, since it has more low speed accuracy self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v) - speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS + self.v_weight * self.v_wheel + speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \ + self.v_weight * self.v_wheel if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_x = [[speed], [0.0]] @@ -254,7 +227,6 @@ class CarState(object): self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change - can_gear_shifter = cp.vl["GEARBOX"]['GEAR_SHIFTER'] self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] @@ -275,7 +247,8 @@ class CarState(object): self.brake_hold = 0 # TODO self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] - self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint) + can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) + self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.shifter_values) self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] # crv doesn't include cruise control diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index be1a29ea00..e9c76d5e46 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -500,7 +500,7 @@ class CarInterface(object): events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse': events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.brake_hold: + if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index ecc0456ac6..67948fdd97 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -124,5 +124,19 @@ STEER_THRESHOLD = { CAR.RIDGELINE: 1200, } +SPEED_FACTOR = { + CAR.ACCORD: 1., + CAR.ACCORD_15: 1., + CAR.ACURA_ILX: 1., + CAR.ACURA_RDX: 1., + CAR.CIVIC: 1., + CAR.CIVIC_HATCH: 1., + CAR.CRV: 1.025, + CAR.CRV_5G: 1.025, + CAR.ODYSSEY: 1., + CAR.PILOT: 1., + CAR.RIDGELINE: 1., +} + # TODO: get these from dbc file HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.CIVIC_HATCH, CAR.CRV_5G] diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 53ff3aaa8a..7e518a57de 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 664167ebce..7022308386 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 f75f4c70a1..473f4504e4 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -511,7 +511,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): prof.checkpoint("State transition") # compute actuators - actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph, + actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 2fd716c3d2..f1c248491e 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 a70eb49845..23e247ed7c 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 df3bb0a967..834f65e502 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 0000000000..fcb78e5fed --- /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 0000000000..c9ff3d604a --- /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 dd8549ff21..1edf82d8e9 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9102036c0e855b6c95c8579a7f9b447a981b4dd505a06cfc403eeb319bc74295 +oid sha256:53a9238e3f5c8906cc0d4bbff573040d65a28010a6fc3ac0273030836ec123e7 size 1626840 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 81a4bd981e..26186f74fc 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 0000000000..829780eb50 --- /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 0000000000..c33a8636bc --- /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 0000000000..298f4fd831 --- /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 0000000000..86a3de6717 --- /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 0000000000..66e4a0f822 --- /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 0000000000..5311d07e7a --- /dev/null +++ b/selfdrive/orbd/dsp/gen/libcalculator_skel.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b94bcedb39d110b074a99fe3129da2506bc4b06cca7f84731cd0620683fd1179 +size 357400 diff --git a/selfdrive/orbd/extractor.h b/selfdrive/orbd/extractor.h new file mode 100644 index 0000000000..f506cd3868 --- /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 0000000000..ce0d47aec6 --- /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 0000000000..8ec7443a30 --- /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 9b4fdd3ad5..57594e9b8a 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9ecc73ffc43883c4407af3c88678262bc0374fb057814fc8b2c09e56619cbe15 +oid sha256:7fbdea9a1cf92975aa4b7506e93dde1e9b8d87f6c2426f98aa7d8831726fb94e size 1171544 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 132f440c61..d812dff08a 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9368e1101d7bcf298761955ae0577aa98235d2f4775278d79d591b4bdcb9f249 +oid sha256:35db92ee0ee547a9bdf8260893abed58cd079ca43d3978a18a16def2a6b66aab size 1159016 diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index bc53a6a41f..464afec307 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 df29fc06ca..e3920cf0ab 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:97110fee2fdc891137a643a7eab38bd269f472658ac05e950fe6dceab306822c -size 12539240 +oid sha256:5f1db707acfb0ef06a15433692d8214695b97ec66ef36a63f74d6b16095a8e35 +size 8775896