diff --git a/.gitignore b/.gitignore index 5c9cfddb98..2f936454dc 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,7 @@ .DS_Store .tags .ipynb_checkpoints +.idea model2.png *.d diff --git a/.gitmodules b/.gitmodules index 6c3b85bd71..60d42b89c5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "opendbc"] path = opendbc url = https://github.com/commaai/opendbc.git +[submodule "pyextra"] + path = pyextra + url = https://github.com/commaai/openpilot-pyextra.git diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 8b1af57b04..3b3638f9ca 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -1,15 +1,9 @@ -FROM ubuntu:14.04 +FROM ubuntu:16.04 ENV PYTHONUNBUFFERED 1 -RUN apt-get update && apt-get install -y build-essential screen wget bzip2 git libglib2.0-0 +RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev -RUN wget -nv -O /tmp/anaconda.sh https://repo.continuum.io/archive/Anaconda2-4.2.0-Linux-x86_64.sh && \ - /bin/bash /tmp/anaconda.sh -b -p /opt/conda && \ - rm /tmp/anaconda.sh - -ENV PATH /opt/conda/bin:$PATH - -RUN conda install numpy==1.11.2 && conda install scipy==0.18.1 +RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib COPY requirements_openpilot.txt /tmp/ RUN pip install -r /tmp/requirements_openpilot.txt diff --git a/README.md b/README.md index a475817409..32513579c4 100644 --- a/README.md +++ b/README.md @@ -101,6 +101,8 @@ Licensing openpilot is released under the MIT license. +Any user of this software shall indemnify and hold harmless Comma.ai, Inc. and its directors, officers, employees, agents, stockholders, affiliates, subcontractors and customers from and against all allegations, claims, actions, suits, demands, damages, liabilities, obligations, losses, settlements, judgments, costs and expenses (including without limitation attorneys’ fees and costs) which arise out of, relate to or result from any use of this software by user. + **THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT. YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS. NO WARRANTY EXPRESSED OR IMPLIED.** diff --git a/RELEASES.md b/RELEASES.md index 860e87696b..874e90978f 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,11 @@ +Version 0.3.4 (2017-07-28) +========================== + * Improved model trained on more data + * Much improved controls tuning + * Performance improvements + * Bugfixes and improvements to calibration + * Driving log can play back video + Version 0.3.3 (2017-06-28) =========================== * Improved model trained on more data diff --git a/apk/com.baseui.apk b/apk/com.baseui.apk index da52e93377..bb3af70b38 100644 --- a/apk/com.baseui.apk +++ b/apk/com.baseui.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:027af259a0b06dc7a719241a74ed9d2d63c13beb49905db1008a908afdbdb1a3 -size 8993895 +oid sha256:31f6d7340f0beed300d7ba8789dcfed8d8fd70aeb815bd430f031587c026b9b2 +size 9479160 diff --git a/cereal/__init__.py b/cereal/__init__.py index fc3071fee9..725c7f8f04 100644 --- a/cereal/__init__.py +++ b/cereal/__init__.py @@ -1,8 +1,20 @@ import os import capnp -capnp.remove_import_hook() CEREAL_PATH = os.path.dirname(os.path.abspath(__file__)) -log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) -car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) +capnp.remove_import_hook() + +if os.getenv("NEWCAPNP"): + import tempfile + import pyximport + importers = pyximport.install(build_dir=os.path.join(tempfile.gettempdir(), ".pyxbld")) + try: + import cereal.gen.cython.log_capnp_cython as log + import cereal.gen.cython.car_capnp_cython as car + finally: + pyximport.uninstall(*importers) + del importers +else: + log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) + car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) diff --git a/cereal/build_from_src.mk b/cereal/build_from_src.mk index 6f523e77ea..37eb8242c6 100644 --- a/cereal/build_from_src.mk +++ b/cereal/build_from_src.mk @@ -1,14 +1,21 @@ SRCS := log.capnp car.capnp -GENS := gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h \ - gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ +GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ + -# Dont build java on the phone... UNAME_M := $(shell uname -m) + +# only generate C++ for docker tests +ifneq ($(OPTEST),1) + GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h + +# Dont build java on the phone... ifeq ($(UNAME_M),x86_64) GENS += gen/java/Car.java gen/java/Log.java endif +endif + .PHONY: all all: $(GENS) diff --git a/cereal/car.capnp b/cereal/car.capnp index 093e66b299..ffdea21e13 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -50,6 +50,7 @@ struct CarState { struct CruiseState { enabled @0: Bool; speed @1: Float32; + mainOn @2: Bool; } enum Error { @@ -63,7 +64,7 @@ struct CarState { seatbeltNotLatched @6; espDisabled @7; wrongCarMode @8; - steerTemporarilyUnavailable @9; + steerTempUnavailable @9; reverseGear @10; # ... } diff --git a/cereal/log.capnp b/cereal/log.capnp index 62ecd1fdd9..fc3dac779a 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -140,12 +140,16 @@ struct SensorEventData { sensor @1 :Int32; type @2 :Int32; timestamp @3 :Int64; + uncalibratedDEPRECATED @10 :Bool; + union { acceleration @4 :SensorVec; magnetic @5 :SensorVec; orientation @6 :SensorVec; gyro @7 :SensorVec; pressure @9 :SensorVec; + magneticUncalibrated @11 :SensorVec; + gyroUncalibrated @12 :SensorVec; } source @8 :SensorSource; @@ -182,7 +186,7 @@ struct GpsLocationData { # Represents heading in degrees. bearing @5 :Float32; - # Represents expected accuracy in meters. + # Represents expected accuracy in meters. (presumably 1 sigma?) accuracy @6 :Float32; # Timestamp for the location fix. @@ -190,6 +194,18 @@ struct GpsLocationData { timestamp @7 :Int64; source @8 :SensorSource; + + # Represents NED velocity in m/s. + vNED @9 :List(Float32); + + # Represents expected vertical accuracy in meters. (presumably 1 sigma?) + verticalAccuracy @10 :Float32; + + # Represents bearing accuracy in degrees. (presumably 1 sigma?) + bearingAccuracy @11 :Float32; + + # Represents velocity accuracy in m/s. (presumably 1 sigma?) + speedAccuracy @12 :Float32; enum SensorSource { android @0; @@ -198,6 +214,7 @@ struct GpsLocationData { velodyne @3; # Velodyne IMU fusion @4; external @5; + ublox @6; } } @@ -205,7 +222,7 @@ struct CanData { address @0 :UInt32; busTime @1 :UInt16; dat @2 :Data; - src @3 :Int8; + src @3 :UInt8; } struct ThermalData { @@ -243,7 +260,8 @@ struct LiveUI { struct Live20Data { canMonoTimes @10 :List(UInt64); mdMonoTime @6 :UInt64; - ftMonoTime @7 :UInt64; + ftMonoTimeDEPRECATED @7 :UInt64; + l100MonoTime @11 :UInt64; # all deprecated warpMatrixDEPRECATED @0 :List(Float32); @@ -296,10 +314,11 @@ struct LiveTracks { } struct Live100Data { - canMonoTime @16 :UInt64; + canMonoTimeDEPRECATED @16 :UInt64; canMonoTimes @21 :List(UInt64); l20MonoTimeDEPRECATED @17 :UInt64; mdMonoTimeDEPRECATED @18 :UInt64; + planMonoTime @28 :UInt64; vEgo @0 :Float32; aEgoDEPRECATED @1 :Float32; @@ -393,7 +412,8 @@ struct EncodeIndex { bigBoxLossless @0; # rcamera.mkv fullHEVC @1; # fcamera.hevc bigBoxHEVC @2; # bcamera.hevc - chffrAndroidH264 @3; # camera + chffrAndroidH264 @3; # acamera + fullLosslessClip @4; # prcamera.mkv } } @@ -413,6 +433,9 @@ struct LogRotate { } struct Plan { + mdMonoTime @9 :UInt64; + l20MonoTime @10 :UInt64; + # lateral, 3rd order polynomial lateralValid @0: Bool; dPoly @1 :List(Float32); @@ -424,6 +447,7 @@ struct Plan { aTargetMax @5 :Float32; jerkFactor @6 :Float32; hasLead @7 :Bool; + fcw @8 :Bool; } struct LiveLocationData { @@ -909,6 +933,114 @@ struct ProcLog { } +struct UbloxGnss { + union { + measurementReport @0 :MeasurementReport; + ephemeris @1 :Ephemeris; + } + + struct MeasurementReport { + #received time of week in gps time in seconds and gps week + rcvTow @0 :Float64; + gpsWeek @1 :UInt16; + # leap seconds in seconds + leapSeconds @2 :UInt16; + # receiver status + receiverStatus @3 :ReceiverStatus; + # num of measurements to follow + numMeas @4 :UInt8; + measurements @5 :List(Measurement); + + struct ReceiverStatus { + # leap seconds have been determined + leapSecValid @0 :Bool; + # Clock reset applied + clkReset @1 : Bool; + } + + struct Measurement { + svId @0 :UInt8; + trackingStatus @1 :TrackingStatus; + # pseudorange in meters + pseudorange @2 :Float64; + # carrier phase measurement in cycles + carrierCycles @3 :Float64; + # doppler measurement in Hz + doppler @4 :Float32; + # GNSS id, 0 is gps + gnssId @5 :UInt8; + glonassFrequencyIndex @6 :UInt8; + # carrier phase locktime counter in ms + locktime @7 :UInt16; + # Carrier-to-noise density ratio (signal strength) in dBHz + cno @8 : UInt8; + # pseudorange standard deviation in meters + pseudorangeStdev @9 :Float32; + # carrier phase standard deviation in cycles + carrierPhaseStdev @10 :Float32; + # doppler standard deviation in Hz + dopplerStdev @11 :Float32; + + struct TrackingStatus { + # pseudorange valid + pseudorangeValid @0 :Bool; + # carrier phase valid + carrierPhaseValid @1 :Bool; + # half cycle valid + halfCycleValid @2 :Bool; + # half sycle subtracted from phase + halfCycleSubtracted @3 :Bool; + } + } + } + + struct Ephemeris { + # This is according to the rinex (2?) format + svId @0 :UInt16; + year @1 :UInt16; + month @2 :UInt16; + day @3 :UInt16; + hour @4 :UInt16; + minute @5 :UInt16; + second @6 :Float32; + af0 @7 :Float64; + af1 @8 :Float64; + af2 @9 :Float64; + + iode @10 :Float64; + crs @11 :Float64; + deltaN @12 :Float64; + m0 @13 :Float64; + + cuc @14 :Float64; + ecc @15 :Float64; + cus @16 :Float64; + a @17 :Float64; # note that this is not the root!! + + toe @18 :Float64; + cic @19 :Float64; + omega0 @20 :Float64; + cis @21 :Float64; + + i0 @22 :Float64; + crc @23 :Float64; + omega @24 :Float64; + omegaDot @25 :Float64; + + iDot @26 :Float64; + codesL2 @27 :Float64; + gpsWeek @28 :Float64; + l2 @29 :Float64; + + svAcc @30 :Float64; + svHealth @31 :Float64; + tgd @32 :Float64; + iodc @33 :Float64; + + transmissionTime @34 :Float64; + fitInterval @35 :Float64; + } +} struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -947,5 +1079,6 @@ struct Event { qcomGnss @31 :QcomGnss; lidarPts @32 :LidarPts; procLog @33 :ProcLog; + ubloxGnss @34 :UbloxGnss; } } diff --git a/common/dbc.py b/common/dbc.py index 9d00062622..518890326b 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -1,6 +1,7 @@ import re -import bitstring +import os import struct +import bitstring from collections import namedtuple def int_or_float(s): @@ -16,6 +17,7 @@ DBCSignal = namedtuple( class dbc(object): def __init__(self, fn): + self.name, _ = os.path.splitext(os.path.basename(fn)) with open(fn) as f: self.txt = f.read().split("\n") self._warned_addresses = set() @@ -32,11 +34,8 @@ class dbc(object): # signals is a list of DBCSignal in order of increasing start_bit. self.msgs = {} - self.bits = [] - for i in range(0, 64, 8): - for j in range(7, -1, -1): - self.bits.append(i+j) - self.bits_index = dict(zip(self.bits, range(64))) + # lookup to bit reverse each byte + self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in xrange(64)] for l in self.txt: l = l.strip() diff --git a/common/fingerprints.py b/common/fingerprints.py index dda045c74b..298424940b 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -1,3 +1,4 @@ +import os _FINGERPRINTS = { "ACURA ILX 2016 ACURAWATCH PLUS": { @@ -51,30 +52,13 @@ def fingerprint(logcan): import selfdrive.messaging as messaging from cereal import car from common.realtime import sec_since_boot - import os - if os.getenv("SIMULATOR") is not None or logcan is None: - # send message - ret = car.CarParams.new_message() - - ret.carName = "simulator" - ret.radarName = "nidec" - ret.carFingerprint = "THE LOW QUALITY SIMULATOR" - - ret.enableSteer = True - ret.enableBrake = True - ret.enableGas = True - ret.enableCruise = False - - ret.wheelBase = 2.67 - ret.steerRatio = 15.3 - ret.slipFactor = 0.0014 - ret.steerKp, ret.steerKi = 12.0, 1.0 - return ret + if os.getenv("SIMULATOR") is not None or logcan is None: + return ("simulator", None) + elif os.getenv("SIMULATOR2") is not None: + return ("simulator2", None) print "waiting for fingerprint..." - brake_only = True - candidate_cars = all_known_cars() finger = {} st = None @@ -83,9 +67,6 @@ def fingerprint(logcan): if st is None: st = sec_since_boot() for can in a.can: - # pedal - if can.address == 0x201 and can.src == 0: - brake_only = False if can.src == 0: finger[can.address] = len(can.dat) candidate_cars = eliminate_incompatible_cars(can, candidate_cars) @@ -98,37 +79,4 @@ def fingerprint(logcan): raise Exception("car doesn't match any fingerprints") print "fingerprinted", candidate_cars[0] - - # send message - ret = car.CarParams.new_message() - - ret.carName = "honda" - ret.radarName = "nidec" - ret.carFingerprint = candidate_cars[0] - - ret.enableSteer = True - ret.enableBrake = True - ret.enableGas = not brake_only - ret.enableCruise = brake_only - #ret.enableCruise = False - - ret.wheelBase = 2.67 - ret.steerRatio = 15.3 - ret.slipFactor = 0.0014 - - if candidate_cars[0] == "HONDA CIVIC 2016 TOURING": - ret.steerKp, ret.steerKi = 12.0, 1.0 - elif candidate_cars[0] == "ACURA ILX 2016 ACURAWATCH PLUS": - if not brake_only: - # assuming if we have an interceptor we also have a torque mod - ret.steerKp, ret.steerKi = 6.0, 0.5 - else: - ret.steerKp, ret.steerKi = 12.0, 1.0 - elif candidate_cars[0] == "HONDA ACCORD 2016 TOURING": - ret.steerKp, ret.steerKi = 12.0, 1.0 - elif candidate_cars[0] == "HONDA CR-V 2016 TOURING": - ret.steerKp, ret.steerKi = 6.0, 0.5 - else: - raise ValueError("unsupported car %s" % candidate_cars[0]) - - return ret + return (candidate_cars[0], finger) diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py index 886ff2ec49..4fe7fc788c 100644 --- a/common/kalman/ekf.py +++ b/common/kalman/ekf.py @@ -1,7 +1,6 @@ import abc import numpy as np import numpy.matlib - # The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use. # A subclass must implement: # 1) calc_transfer_fun(); see bottom of file for more info. @@ -68,55 +67,6 @@ class SimpleSensor: self.covar = covar return SensorReading(data, self.covar, self.obs_model) -class GPS: - earth_r = 6371e3 # m, average earth radius - - def __init__(self, xy_idx=(0, 1), dims=2, var=1e4): - self.obs_model = np.matlib.zeros((2, dims)) - self.obs_model[:, tuple(xy_idx)] = np.matlib.identity(2) - self.covar = np.matlib.identity(2) * var - - # [lat, lon] in decimal degrees - def init_pos(self, latlon): - self.init_lat, self.init_lon = np.radians(np.asarray(latlon[:2])) - - # Compute straight-line distance, in meters, between two lat/long coordinates - # Input in radians - def haversine(self, lat1, lon1, lat2, lon2): - lat_diff = lat2 - lat1 - lon_diff = lon2 - lon1 - d = np.sin(lat_diff * 0.5)**2 + np.cos(lat1) * np.cos(lat2) * np.sin( - lon_diff * 0.5)**2 - h = 2 * GPS.earth_r * np.arcsin(np.sqrt(d)) - return h - - # Convert decimal degrees into meters - def convert_deg2m(self, lat, lon): - lat, lon = np.radians([lat, lon]) - - xs = (lon - self.init_lon) * np.cos(self.init_lat) * GPS.earth_r - ys = (lat - self.init_lat) * GPS.earth_r - - return xs, ys - - # Convert meters into decimal degrees - def convert_m2deg(self, xs, ys): - lat = ys / GPS.earth_r + self.init_lat - lon = xs / (GPS.earth_r * np.cos(self.init_lat)) + self.init_lon - - return np.degrees(lat), np.degrees(lon) - - # latlon is [lat, long,] as decimal degrees - # accuracy is as given by Android location service: radius of 68% confidence - def read(self, latlon, accuracy=None): - x_dist, y_dist = self.convert_deg2m(latlon[0], latlon[1]) - if not accuracy: - covar = self.covar - else: - covar = np.matlib.identity(2) * accuracy**2 - - return SensorReading( - np.asmatrix([x_dist, y_dist]).T, covar, self.obs_model) class EKF: __metaclass__ = abc.ABCMeta @@ -224,7 +174,7 @@ class EKF: #! Clip covariance to avoid explosions self.covar = np.clip(self.covar,-1e10,1e10) - + @abc.abstractmethod def calc_transfer_fun(self, dt): """Return a tuple with the transfer function and transfer function jacobian diff --git a/common/logging_extra.py b/common/logging_extra.py index 272fc23214..359b3e0914 100644 --- a/common/logging_extra.py +++ b/common/logging_extra.py @@ -16,6 +16,10 @@ def json_handler(obj): def json_robust_dumps(obj): return json.dumps(obj, default=json_handler) +class NiceOrderedDict(OrderedDict): + def __str__(self): + return '{'+', '.join("%r: %r" % p for p in self.iteritems())+'}' + class SwagFormatter(logging.Formatter): def __init__(self, swaglogger): logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y') @@ -24,7 +28,7 @@ class SwagFormatter(logging.Formatter): self.host = socket.gethostname() def format_dict(self, record): - record_dict = OrderedDict() + record_dict = NiceOrderedDict() if isinstance(record.msg, dict): record_dict['msg'] = record.msg @@ -119,7 +123,7 @@ class SwagLogger(logging.Logger): self.global_ctx.update(kwargs) def event(self, event_name, *args, **kwargs): - evt = OrderedDict() + evt = NiceOrderedDict() evt['event'] = event_name if args: evt['args'] = args diff --git a/common/realtime.py b/common/realtime.py index 9315f2196e..e84e6eb490 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -29,7 +29,7 @@ libc = ffi.dlopen(None) CLOCK_MONOTONIC_RAW = 4 CLOCK_BOOTTIME = 7 -if hasattr(libc, 'clock_gettime'): +if platform.system() != 'Darwin' and hasattr(libc, 'clock_gettime'): c_clock_gettime = libc.clock_gettime tlocal = threading.local() diff --git a/opendbc b/opendbc index d584cdd46c..b77861eb00 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit d584cdd46ca398aace9f1a7ee6c967a8c7602e8e +Subproject commit b77861eb00d45e25af501f78bbed155d2df06159 diff --git a/pyextra b/pyextra new file mode 160000 index 0000000000..e0738376db --- /dev/null +++ b/pyextra @@ -0,0 +1 @@ +Subproject commit e0738376db27d208603d7e63dd465e003ca06325 diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt index 717a09dedf..7d2c68452d 100644 --- a/requirements_openpilot.txt +++ b/requirements_openpilot.txt @@ -9,4 +9,6 @@ requests==2.10.0 setproctitle==1.1.10 simplejson==3.8.2 pyyaml==3.12 +cffi==1.7.0 +enum34==1.1.1 -e git+https://github.com/commaai/le_python.git#egg=Logentries diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore new file mode 100644 index 0000000000..b030dc97cd --- /dev/null +++ b/selfdrive/boardd/.gitignore @@ -0,0 +1 @@ +boardd diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 05181b9c62..a2390ac02d 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -31,11 +31,16 @@ pthread_mutex_t usb_lock; bool spoofing_started = false; bool fake_send = false; +bool loopback_can = false; // double the FIFO size #define RECV_SIZE (0x1000) #define TIMEOUT 0 +#define SAFETY_NOOUTPUT 0x0000 +#define SAFETY_HONDA 0x0001 +#define SAFETY_ALLOUTPUT 0x1337 + bool usb_connect() { int err; @@ -48,22 +53,36 @@ bool usb_connect() { err = libusb_claim_interface(dev_handle, 0); if (err != 0) { return false; } + if (loopback_can) { + libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT); + } + // power off ESP libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT); // forward CAN1 to CAN3...soon //libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT); - + // set UART modes for Honda Accord - for (int uart = 2; uart <= 3; uart++) { + /*for (int uart = 2; uart <= 3; uart++) { // 9600 baud - libusb_control_transfer(dev_handle, 0xc0, 0xe1, uart, 9600, NULL, 0, TIMEOUT); + libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT); // even parity - libusb_control_transfer(dev_handle, 0xc0, 0xe2, uart, 1, NULL, 0, TIMEOUT); + libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT); // callback 1 - libusb_control_transfer(dev_handle, 0xc0, 0xe3, uart, 1, NULL, 0, TIMEOUT); + libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT); } + // TODO: Boardd should be able to set the baud rate + int baud = 500000; + libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0, + (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1 + libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0, + (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/ + + // TODO: Boardd should be able to be told which safety model to use + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT); + return true; } @@ -90,7 +109,7 @@ void can_recv(void *s) { // do recv pthread_mutex_lock(&usb_lock); - + do { err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT); if (err != 0) { handle_usb_issue(err, __func__); } @@ -127,13 +146,13 @@ void can_recv(void *s) { canData[i].setBusTime(data[i*4+1] >> 16); int len = data[i*4+1]&0xF; canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len)); - canData[i].setSrc((data[i*4+1] >> 4) & 0xf); + canData[i].setSrc((data[i*4+1] >> 4) & 0xff); } // send to can auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); - zmq_send(s, bytes.begin(), bytes.size(), 0); + zmq_send(s, bytes.begin(), bytes.size(), 0); } void can_health(void *s) { @@ -181,7 +200,7 @@ void can_health(void *s) { // send to health auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); - zmq_send(s, bytes.begin(), bytes.size(), 0); + zmq_send(s, bytes.begin(), bytes.size(), 0); } @@ -316,6 +335,10 @@ int main() { fake_send = true; } + if(getenv("BOARDD_LOOPBACK")){ + loopback_can = true; + } + // init libusb err = libusb_init(&ctx); assert(err == 0); @@ -357,4 +380,3 @@ int main() { libusb_close(dev_handle); libusb_exit(ctx); } - diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 16715119bb..8bda67923f 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -18,6 +18,10 @@ except Exception: # TODO: rewrite in C to save CPU +SAFETY_NOOUTPUT = 0 +SAFETY_HONDA = 1 +SAFETY_ALLOUTPUT = 0x1337 + # *** serialization functions *** def can_list_to_can_capnp(can_msgs, msgtype='can'): dat = messaging.new_message() @@ -85,6 +89,7 @@ def can_recv(): def can_init(): global handle, context + handle = None cloudlog.info("attempting can init") context = usb1.USBContext() @@ -94,6 +99,7 @@ def can_init(): if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc: handle = device.open() handle.claimInterface(0) + handle.controlWrite(0x40, 0xdc, SAFETY_HONDA, 0, b'') if handle is None: print "CAN NOT FOUND" @@ -190,4 +196,3 @@ def main(gctx=None): if __name__ == "__main__": main() - diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile new file mode 100644 index 0000000000..9c2ebbd732 --- /dev/null +++ b/selfdrive/can/Makefile @@ -0,0 +1,72 @@ +CC = clang +CXX = clang++ + +PHONELIBS := ../../phonelibs + +UNAME_S := $(shell uname -s) +UNAME_M := $(shell uname -m) + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args \ + -Wno-deprecated-declarations + +CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) + +ifeq ($(UNAME_S),Darwin) + CEREAL_LIBS := -L /usr/local/lib -lkj -lcapnp + ZMQ_LIBS = -L/usr/local/lib -lzmq +else ifeq ($(OPTEST),1) + ZMQ_LIBS = -lzmq + CEREAL_LIBS = -lcapnp -lkj +else ifeq ($(UNAME_M),x86_64) + EXTERNAL := ../../external + ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include + ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a + CEREAL_CXXFLAGS := -I$(EXTERNAL)/capnp/include + CEREAL_LIBS := -L$(EXTERNAL)/capnp/lib -l:libcapnp.a -l:libkj.a +else ifeq ($(UNAME_M),aarch64) + ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include + ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a +endif + +OPENDBC_PATH := $(shell python -c 'import opendbc; print opendbc.DBC_PATH') + +DBC_SOURCES := $(wildcard $(OPENDBC_PATH)/*.dbc) +DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES)) + +CWD := $(shell pwd) + +.PHONY: all +all: libdbc.so + +ifeq ($(UNAME_M),aarch64) +include ../common/cereal.mk +endif + +# make sure cereal is built +libdbc.so:: ../../cereal/gen/cpp/log.capnp.h + +../../cereal/gen/cpp/log.capnp.h: + cd ../../cereal && make + +libdbc.so:: parser.cc $(DBC_CCS) + $(CXX) -fPIC -shared -o '$@' $^ \ + -I. \ + -I../.. \ + $(CXXFLAGS) \ + $(ZMQ_FLAGS) \ + $(ZMQ_LIBS) \ + $(CEREAL_CXXFLAGS) \ + $(CEREAL_LIBS) + +dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc + PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@' + +.PHONY: clean +clean: + rm -rf libdbc.so* + rm -f dbc_out/*.cc diff --git a/selfdrive/can/__init__.py b/selfdrive/can/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/can/dbc_out/.gitignore b/selfdrive/can/dbc_out/.gitignore new file mode 100644 index 0000000000..4625581a85 --- /dev/null +++ b/selfdrive/can/dbc_out/.gitignore @@ -0,0 +1,2 @@ +*.cc + diff --git a/selfdrive/can/dbc_out/.gitkeep b/selfdrive/can/dbc_out/.gitkeep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc new file mode 100644 index 0000000000..3676c823ec --- /dev/null +++ b/selfdrive/can/dbc_template.cc @@ -0,0 +1,51 @@ +#include + +#include "parser_common.h" + +namespace { + +{% for address, msg_name, sigs in msgs %} +const Signal sigs_{{address}}[] = { + {% for sig in sigs %} + { + {% set b1 = (sig.start_bit//8)*8 + (-sig.start_bit-1) % 8 %} + .name = "{{sig.name}}", + .b1 = {{b1}}, + .b2 = {{sig.size}}, + .bo = {{64 - (b1 + sig.size)}}, + .is_signed = {{"true" if sig.is_signed else "false"}}, + .factor = {{sig.factor}}, + .offset = {{sig.offset}}, + {% if checksum_type == "honda" and sig.name == "CHECKSUM" %} + .type = SignalType::HONDA_CHECKSUM, + {% elif checksum_type == "honda" and sig.name == "COUNTER" %} + .type = SignalType::HONDA_COUNTER, + {% else %} + .type = SignalType::DEFAULT, + {% endif %} + }, + {% endfor %} +}; +{% endfor %} + +const Msg msgs[] = { +{% for address, msg_name, sigs in msgs %} + {% set address_hex = "0x%X" % address %} + { + .name = "{{msg_name}}", + .address = {{address_hex}}, + .num_sigs = ARRAYSIZE(sigs_{{address}}), + .sigs = sigs_{{address}}, + }, +{% endfor %} +}; + +} + +const DBC {{dbc.name}} = { + .name = "{{dbc.name}}", + .num_msgs = ARRAYSIZE(msgs), + .msgs = msgs, +}; + +dbc_init({{dbc.name}}) diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc new file mode 100644 index 0000000000..8507a5eebf --- /dev/null +++ b/selfdrive/can/parser.cc @@ -0,0 +1,441 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "parser_common.h" + +#define DEBUG(...) +// #define DEBUG printf +#define INFO printf + + +#define MAX_BAD_COUNTER 5 + +namespace { + +uint64_t read_u64_be(const uint8_t* v) { + return (((uint64_t)v[0] << 56) + | ((uint64_t)v[1] << 48) + | ((uint64_t)v[2] << 40) + | ((uint64_t)v[3] << 32) + | ((uint64_t)v[4] << 24) + | ((uint64_t)v[5] << 16) + | ((uint64_t)v[6] << 8) + | (uint64_t)v[7]); +} + +std::vector g_dbc; + +bool honda_checksum(int address, uint64_t d, int l) { + int target = (d >> l) & 0xF; + + DEBUG("check checksum %16lx %d", d, l); + + // remove checksum from calculation + d &= ~(0xFLL << l); + + int s = 0; + while (address > 0) { s += (address & 0xF); address >>= 4; } + while (d > 0) { s += (d & 0xF); d >>= 4; } + s = 8-s; + s &= 0xF; + + DEBUG(" %d = %d\n", target, s); + return target == s; +} + +struct MessageState { + uint32_t address; + + std::vector parse_sigs; + std::vector vals; + + uint64_t seen; + uint64_t check_threshold; + + uint8_t counter; + uint8_t counter_fail; + + bool parse(uint64_t sec, uint64_t dat) { + for (int i=0; i < parse_sigs.size(); i++) { + auto& sig = parse_sigs[i]; + + int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1); + if (sig.is_signed) { + tmp -= (tmp >> (sig.b2-1)) ? (1< %ld\n", address, sig.name, tmp); + + if (sig.type == SignalType::HONDA_CHECKSUM) { + if (!honda_checksum(address, dat, sig.bo)) { + INFO("%X CHECKSUM FAIL\n", address); + return false; + } + } else if (sig.type == SignalType::HONDA_COUNTER) { + if (!honda_update_counter(tmp)) { + return false; + } + } + + vals[i] = tmp * sig.factor + sig.offset; + } + seen = sec; + + return true; + } + + + bool honda_update_counter(int64_t v) { + uint8_t old_counter = counter; + counter = v; + if (((old_counter+1) & 3) != v) { + counter_fail += 1; + if (counter_fail > 1) { + INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v); + } + if (counter_fail >= MAX_BAD_COUNTER) { + return false; + } + } else if (counter_fail > 0) { + counter_fail--; + } + return true; + } + +}; + +class CANParser { + public: + CANParser(int abus, const std::string& dbc_name, + const std::vector &options, + const std::vector &sigoptions) + : bus(abus) { + // connect to can on 8006 + context = zmq_ctx_new(); + subscriber = zmq_socket(context, ZMQ_SUB); + zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); + zmq_connect(subscriber, "tcp://127.0.0.1:8006"); + + for (auto dbci : g_dbc) { + if (dbci->name == dbc_name) { + dbc = dbci; + break; + } + } + assert(dbc); + + for (auto &op : options) { + MessageState state = { + .address = op.address, + // .check_frequency = op.check_frequency, + }; + + // msg is not valid if a message isn't received for 10 consecutive steps + if (op.check_frequency > 0) { + state.check_threshold = (1000000000ULL / op.check_frequency) * 10; + } + + + const Msg* msg = NULL; + for (int i=0; inum_msgs; i++) { + if (dbc->msgs[i].address == op.address) { + msg = &dbc->msgs[i]; + break; + } + } + if (!msg) { + fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str()); + assert(false); + } + + // track checksums and for this message + for (int i=0; inum_sigs; i++) { + const Signal *sig = &msg->sigs[i]; + if (sig->type == HONDA_CHECKSUM + || sig->type == HONDA_COUNTER) { + state.parse_sigs.push_back(*sig); + state.vals.push_back(0); + } + } + + // track requested signals for this message + for (auto &sigop : sigoptions) { + if (sigop.address != op.address) continue; + + for (int i=0; inum_sigs; i++) { + const Signal *sig = &msg->sigs[i]; + if (strcmp(sig->name, sigop.name) == 0 + && sig->type != HONDA_CHECKSUM + && sig->type != HONDA_COUNTER) { + state.parse_sigs.push_back(*sig); + state.vals.push_back(sigop.default_value); + break; + } + } + + } + + message_states[state.address] = state; + } + } + + void UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { + int msg_count = cans.size(); + + DEBUG("got %d messages\n", msg_count); + + // parse the messages + for (int i = 0; i < msg_count; i++) { + auto cmsg = cans[i]; + if (cmsg.getSrc() != bus) { + // DEBUG("skip %d: wrong bus\n", cmsg.getAddress()); + continue; + } + auto state_it = message_states.find(cmsg.getAddress()); + if (state_it == message_states.end()) { + // DEBUG("skip %d: not specified\n", cmsg.getAddress()); + continue; + } + + if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen + uint8_t dat[8] = {0}; + memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); + + uint64_t p = read_u64_be(dat); + + DEBUG(" proc %X: %lx\n", cmsg.getAddress(), p); + + state_it->second.parse(sec, p); + } + } + + void UpdateValid(uint64_t sec) { + can_valid = true; + for (auto &kv : message_states) { + auto &state = kv.second; + if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { + if (state.seen > 0) { + INFO("%X TIMEOUT\n", state.address); + } + can_valid = false; + } + } + } + + void update(uint64_t sec, bool wait) { + int err; + + // recv from can + zmq_msg_t msg; + zmq_msg_init(&msg); + + // multiple recv is fine + bool first = wait; + while (1) { + if (first) { + err = zmq_msg_recv(&msg, subscriber, 0); + first = false; + } else { + err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT); + } + if (err < 0) break; + + // format for board, make copy due to alignment issues, will be freed on out of scope + auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); + + // extract the messages + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + auto cans = event.getCan(); + + UpdateCans(sec, cans); + } + + UpdateValid(sec); + + } + + std::vector query(uint64_t sec) { + std::vector ret; + + for (auto &kv : message_states) { + auto &state = kv.second; + if (sec != 0 && state.seen != sec) continue; + + for (int i=0; i message_states; +}; + +} + +void dbc_register(const DBC* dbc) { + g_dbc.push_back(dbc); +} + +extern "C" { + +void* can_init(int bus, const char* dbc_name, + size_t num_message_options, const MessageParseOptions* message_options, + size_t num_signal_options, const SignalParseOptions* signal_options) { + CANParser* ret = new CANParser(bus, std::string(dbc_name), + (message_options ? std::vector(message_options, message_options+num_message_options) + : std::vector{}), + (signal_options ? std::vector(signal_options, signal_options+num_signal_options) + : std::vector{})); + return (void*)ret; +} + +void can_update(void* can, uint64_t sec, bool wait) { + CANParser* cp = (CANParser*)can; + cp->update(sec, wait); +} + +size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) { + CANParser* cp = (CANParser*)can; + + if (out_can_valid) { + *out_can_valid = cp->can_valid; + } + + const std::vector values = cp->query(sec); + if (out_values) { + std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values); + } + return values.size(); +}; + +} + +#ifdef TEST + +int main(int argc, char** argv) { + CANParser cp(0, "honda_civic_touring_2016_can", + std::vector{ + // address, check_frequency + {0x14a, 100}, + {0x158, 100}, + {0x17c, 100}, + {0x191, 100}, + {0x1a4, 50}, + {0x326, 10}, + {0x1b0, 50}, + {0x1d0, 50}, + {0x305, 10}, + {0x324, 10}, + {0x405, 3}, + {0x18f, 0}, + {0x130, 0}, + {0x296, 0}, + {0x30c, 0}, + }, + std::vector{ + // sig_name, sig_address, default + {0x158, "XMISSION_SPEED", 0}, + {0x1d0, "WHEEL_SPEED_FL", 0}, + {0x1d0, "WHEEL_SPEED_FR", 0}, + {0x1d0, "WHEEL_SPEED_RL", 0}, + {0x14a, "STEER_ANGLE", 0}, + {0x18f, "STEER_TORQUE_SENSOR", 0}, + {0x191, "GEAR", 0}, + {0x1b0, "WHEELS_MOVING", 1}, + {0x405, "DOOR_OPEN_FL", 1}, + {0x405, "DOOR_OPEN_FR", 1}, + {0x405, "DOOR_OPEN_RL", 1}, + {0x405, "DOOR_OPEN_RR", 1}, + {0x324, "CRUISE_SPEED_PCM", 0}, + {0x305, "SEATBELT_DRIVER_LAMP", 1}, + {0x305, "SEATBELT_DRIVER_LATCHED", 0}, + {0x17c, "BRAKE_PRESSED", 0}, + {0x130, "CAR_GAS", 0}, + {0x296, "CRUISE_BUTTONS", 0}, + {0x1a4, "ESP_DISABLED", 1}, + {0x30c, "HUD_LEAD", 0}, + {0x1a4, "USER_BRAKE", 0}, + {0x18f, "STEER_STATUS", 5}, + {0x1d0, "WHEEL_SPEED_RR", 0}, + {0x1b0, "BRAKE_ERROR_1", 1}, + {0x1b0, "BRAKE_ERROR_2", 1}, + {0x191, "GEAR_SHIFTER", 0}, + {0x326, "MAIN_ON", 0}, + {0x17c, "ACC_STATUS", 0}, + {0x17c, "PEDAL_GAS", 0}, + {0x296, "CRUISE_SETTING", 0}, + {0x326, "LEFT_BLINKER", 0}, + {0x326, "RIGHT_BLINKER", 0}, + {0x324, "COUNTER", 0}, + {0x17c, "ENGINE_RPM", 0}, + }); + + + + const std::string log_fn = "dats.bin"; + + int log_fd = open(log_fn.c_str(), O_RDONLY, 0); + assert(log_fd >= 0); + + off_t log_size = lseek(log_fd, 0, SEEK_END); + lseek(log_fd, 0, SEEK_SET); + + void* log_data = mmap(NULL, log_size, PROT_READ, MAP_PRIVATE, log_fd, 0); + assert(log_data); + + auto words = kj::arrayPtr((const capnp::word*)log_data, log_size/sizeof(capnp::word)); + while (words.size() > 0) { + capnp::FlatArrayMessageReader reader(words); + + auto evt = reader.getRoot(); + auto cans = evt.getCan(); + + cp.UpdateCans(0, cans); + + words = kj::arrayPtr(reader.getEnd(), words.end()); + } + + munmap(log_data, log_size); + + close(log_fd); + + return 0; +} + +#endif diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py new file mode 100644 index 0000000000..4a9490c772 --- /dev/null +++ b/selfdrive/can/parser.py @@ -0,0 +1,166 @@ +import os +import time +import subprocess +from collections import defaultdict + +from cffi import FFI + +can_dir = os.path.dirname(os.path.abspath(__file__)) +libdbc_fn = os.path.join(can_dir, "libdbc.so") +subprocess.check_output(["make"], cwd=can_dir) + +ffi = FFI() +ffi.cdef(""" + +typedef struct SignalParseOptions { + uint32_t address; + const char* name; + double default_value; +} SignalParseOptions; + +typedef struct MessageParseOptions { + uint32_t address; + int check_frequency; +} MessageParseOptions; + +typedef struct SignalValue { + uint32_t address; + const char* name; + double value; +} SignalValue; + +void* can_init(int bus, const char* dbc_name, + size_t num_message_options, const MessageParseOptions* message_options, + size_t num_signal_options, const SignalParseOptions* signal_options); + +void can_update(void* can, uint64_t sec, bool wait); + +size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values); + +""") + +libdbc = ffi.dlopen(libdbc_fn) + +class CANParser(object): + def __init__(self, dbc_name, signals, checks=[], bus=0): + self.can_valid = True + self.vl = defaultdict(dict) + + sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals) + + signal_options_c = ffi.new("SignalParseOptions[]", [ + { + 'address': sig_address, + 'name': sig_names[sig_name], + 'default_value': sig_default, + } for sig_name, sig_address, sig_default in signals]) + + message_options = dict((address, 0) for _, address, _ in signals) + message_options.update(dict(checks)) + + message_options_c = ffi.new("MessageParseOptions[]", [ + { + 'address': address, + 'check_frequency': freq, + } for address, freq in message_options.iteritems()]) + + self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c, + len(signal_options_c), signal_options_c) + + self.p_can_valid = ffi.new("bool*") + + value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL) + self.can_values = ffi.new("SignalValue[%d]" % value_count) + self.update_vl(0) + # print "===" + + def update_vl(self, sec): + + can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values) + assert can_values_len <= len(self.can_values) + + self.can_valid = self.p_can_valid[0] + + # print can_values_len + ret = set() + for i in xrange(can_values_len): + cv = self.can_values[i] + address = cv.address + # print hex(cv.address), ffi.string(cv.name) + self.vl[address][ffi.string(cv.name)] = cv.value + ret.add(address) + return ret + + def update(self, sec, wait): + libdbc.can_update(self.can, sec, wait) + return self.update_vl(sec) + +if __name__ == "__main__": + from common.realtime import sec_since_boot + + radar_messages = range(0x430, 0x43A) + range(0x440, 0x446) + # signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + + # ['REL_SPEED'] * 16, radar_messages * 4, + # [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) + # checks = zip(radar_messages, [20]*16) + + # cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1) + + signals = [ + ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default + ("WHEEL_SPEED_FL", 0x1d0, 0), + ("WHEEL_SPEED_FR", 0x1d0, 0), + ("WHEEL_SPEED_RL", 0x1d0, 0), + ("STEER_ANGLE", 0x14a, 0), + ("STEER_TORQUE_SENSOR", 0x18f, 0), + ("GEAR", 0x191, 0), + ("WHEELS_MOVING", 0x1b0, 1), + ("DOOR_OPEN_FL", 0x405, 1), + ("DOOR_OPEN_FR", 0x405, 1), + ("DOOR_OPEN_RL", 0x405, 1), + ("DOOR_OPEN_RR", 0x405, 1), + ("CRUISE_SPEED_PCM", 0x324, 0), + ("SEATBELT_DRIVER_LAMP", 0x305, 1), + ("SEATBELT_DRIVER_LATCHED", 0x305, 0), + ("BRAKE_PRESSED", 0x17c, 0), + ("CAR_GAS", 0x130, 0), + ("CRUISE_BUTTONS", 0x296, 0), + ("ESP_DISABLED", 0x1a4, 1), + ("HUD_LEAD", 0x30c, 0), + ("USER_BRAKE", 0x1a4, 0), + ("STEER_STATUS", 0x18f, 5), + ("WHEEL_SPEED_RR", 0x1d0, 0), + ("BRAKE_ERROR_1", 0x1b0, 1), + ("BRAKE_ERROR_2", 0x1b0, 1), + ("GEAR_SHIFTER", 0x191, 0), + ("MAIN_ON", 0x326, 0), + ("ACC_STATUS", 0x17c, 0), + ("PEDAL_GAS", 0x17c, 0), + ("CRUISE_SETTING", 0x296, 0), + ("LEFT_BLINKER", 0x326, 0), + ("RIGHT_BLINKER", 0x326, 0), + ("COUNTER", 0x324, 0), + ("ENGINE_RPM", 0x17C, 0) + ] + checks = [ + (0x14a, 100), # address, frequency + (0x158, 100), + (0x17c, 100), + (0x191, 100), + (0x1a4, 50), + (0x326, 10), + (0x1b0, 50), + (0x1d0, 50), + (0x305, 10), + (0x324, 10), + (0x405, 3), + ] + + cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0) + print cp.vl + + while True: + cp.update(int(sec_since_boot()*1e9), True) + print cp.vl + print cp.can_valid + time.sleep(0.01) diff --git a/selfdrive/can/parser_common.h b/selfdrive/can/parser_common.h new file mode 100644 index 0000000000..34676a690c --- /dev/null +++ b/selfdrive/can/parser_common.h @@ -0,0 +1,63 @@ +#ifndef PARSER_COMMON_H +#define PARSER_COMMON_H + +#include +#include + +#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) + + + +struct SignalParseOptions { + uint32_t address; + const char* name; + double default_value; +}; + +struct MessageParseOptions { + uint32_t address; + int check_frequency; +}; + +struct SignalValue { + uint32_t address; + const char* name; + double value; +}; + + +enum SignalType { + DEFAULT, + HONDA_CHECKSUM, + HONDA_COUNTER, +}; + +struct Signal { + const char* name; + int b1, b2, bo; + bool is_signed; + double factor, offset; + SignalType type; +}; + +struct Msg { + const char* name; + uint32_t address; + size_t num_sigs; + const Signal *sigs; +}; + +struct DBC { + const char* name; + size_t num_msgs; + const Msg *msgs; +}; + +void dbc_register(const DBC* dbc); + +#define dbc_init(dbc) \ +static void __attribute__((constructor)) do_dbc_init_ ## dbc(void) { \ + dbc_register(&dbc); \ +} + +#endif diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py new file mode 100755 index 0000000000..81646f78f9 --- /dev/null +++ b/selfdrive/can/process_dbc.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +import os +import sys + +import jinja2 + +import opendbc +from common.dbc import dbc + +if len(sys.argv) != 3: + print "usage: %s dbc_path struct_path" % (sys.argv[0],) + sys.exit(0) + +dbc_fn = sys.argv[1] +out_fn = sys.argv[2] + +template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc") + +can_dbc = dbc(dbc_fn) + +with open(template_fn, "r") as template_f: + template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) + +msgs = [(address, msg_name, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first + for address, ((msg_name, _), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs] + +checksum_type = "honda" if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura") else None + +parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len) + +with open(out_fn, "w") as out_f: + out_f.write(parser_code) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index e69de29bb2..e18afe12da 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -0,0 +1,30 @@ +from common.fingerprints import fingerprint + +from .honda.interface import CarInterface as HondaInterface + +try: + from .simulator.interface import CarInterface as SimInterface +except ImportError: + SimInterface = None + +try: + from .simulator2.interface import CarInterface as Sim2Interface +except ImportError: + Sim2Interface = None + +interfaces = { + "HONDA CIVIC 2016 TOURING": HondaInterface, + "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface, + "HONDA ACCORD 2016 TOURING": HondaInterface, + "HONDA CR-V 2016 TOURING": HondaInterface, + + "simulator": SimInterface, + "simulator2": Sim2Interface +} + +def get_car(logcan, sendcan=None): + candidate, fingerprints = fingerprint(logcan) + interface_cls = interfaces[candidate] + params = interface_cls.get_params(candidate, fingerprints) + + return interface_cls(params, logcan, sendcan), params diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 4beecc22de..71783210d4 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,11 +1,14 @@ from collections import namedtuple import common.numpy_fast as np +from common.numpy_fast import clip, interp from common.realtime import sec_since_boot + from selfdrive.config import CruiseButtons from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.controls.lib.drive_helpers import rate_limit -from common.numpy_fast import clip, interp +from . import hondacan + def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic): # hyst params... TODO: move these to VehicleParams @@ -66,7 +69,6 @@ def process_hud_alert(hud_alert): return fcw_display, steer_required, acc_alert -import selfdrive.car.honda.hondacan as hondacan HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5", @@ -126,7 +128,7 @@ class CarController(object): #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) - hud = HUDData(int(pcm_accel), int(hud_v_cruise), 0x01, hud_car, + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 0x01, hud_car, 0xc1, 0x41, hud_lanes + steer_required, int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 0e8f5a4f2c..c4cf058c40 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,15 +1,22 @@ +import os +import time + import common.numpy_fast as np +from common.realtime import sec_since_boot import selfdrive.messaging as messaging -from common.realtime import sec_since_boot from selfdrive.car.honda.can_parser import CANParser +from selfdrive.can.parser import CANParser as CANParserC + +NEW_CAN = os.getenv("OLD_CAN") is None def get_can_parser(CP): # this function generates lists for signal, messages and initial values if CP.carFingerprint == "HONDA CIVIC 2016 TOURING": dbc_f = 'honda_civic_touring_2016_can.dbc' signals = [ + # sig_name, sig_address, default ("XMISSION_SPEED", 0x158, 0), ("WHEEL_SPEED_FL", 0x1d0, 0), ("WHEEL_SPEED_FR", 0x1d0, 0), @@ -46,6 +53,7 @@ def get_can_parser(CP): ("ENGINE_RPM", 0x17C, 0) ] checks = [ + # address, frequency (0x14a, 100), (0x158, 100), (0x17c, 100), @@ -219,7 +227,10 @@ def get_can_parser(CP): signals.append(("INTERCEPTOR_GAS", 0x201, 0)) checks.append((0x201, 50)) - return CANParser(dbc_f, signals, checks) + if NEW_CAN: + return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0) + else: + return CANParser(dbc_f, signals, checks) class CarState(object): def __init__(self, CP, logcan): @@ -256,9 +267,12 @@ class CarState(object): # TODO: actually make this work self.a_ego = 0. - def update(self, can_pub_main): + def update(self, can_pub_main=None): cp = self.cp - cp.update_can(can_pub_main) + if NEW_CAN: + cp.update(int(sec_since_boot() * 1e9), False) + else: + cp.update_can(can_pub_main) # copy can_valid self.can_valid = cp.can_valid diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 0eb171187c..1832f3b68b 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,18 +1,20 @@ #!/usr/bin/env python +import os import time import common.numpy_fast as np from selfdrive.config import Conversions as CV -from selfdrive.car.honda.carstate import CarState -from selfdrive.car.honda.carcontroller import CarController, AH +from .carstate import CarState +from .carcontroller import CarController, AH from selfdrive.boardd.boardd import can_capnp_to_can_list from cereal import car -import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging +NEW_CAN = os.getenv("OLD_CAN") is None + # Car button codes class CruiseButtons: RES_ACCEL = 4 @@ -35,7 +37,6 @@ class BP: TRIPLE = 2 REPEATED = 1 - class CarInterface(object): def __init__(self, CP, logcan, sendcan=None): self.logcan = logcan @@ -55,19 +56,65 @@ class CarInterface(object): if self.CS.accord: self.accord_msg = [] + @staticmethod + def get_params(candidate, fingerprint): + + # pedal + brake_only = 0x201 not in fingerprint + + ret = car.CarParams.new_message() + + ret.carName = "honda" + ret.radarName = "nidec" + ret.carFingerprint = candidate + + ret.enableSteer = True + ret.enableBrake = True + ret.enableGas = not brake_only + ret.enableCruise = brake_only + #ret.enableCruise = False + + # TODO: those parameters should be platform dependent + ret.wheelBase = 2.67 + ret.slipFactor = 0.0014 + + if candidate == "HONDA CIVIC 2016 TOURING": + ret.steerRatio = 13.0 + ret.steerKp, ret.steerKi = 6.0, 1.4 + elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS": + ret.steerRatio = 15.3 + if not brake_only: + # assuming if we have an interceptor we also have a torque mod + ret.steerKp, ret.steerKi = 3.0, 0.7 + else: + ret.steerKp, ret.steerKi = 6.0, 1.4 + elif candidate == "HONDA ACCORD 2016 TOURING": + ret.steerRatio = 15.3 + ret.steerKp, ret.steerKi = 6.0, 1.4 + elif candidate == "HONDA CR-V 2016 TOURING": + ret.steerRatio = 15.3 + ret.steerKp, ret.steerKi = 3.0, 0.7 + else: + raise ValueError("unsupported car %s" % candidate) + + return ret + # returns a car.CarState def update(self): # ******************* do can recv ******************* can_pub_main = [] canMonoTimes = [] - for a in messaging.drain_sock(self.logcan): - canMonoTimes.append(a.logMonoTime) - can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2])) - if self.CS.accord: - self.accord_msg.extend(can_capnp_to_can_list(a.can, [9])) - self.accord_msg = self.accord_msg[-1:] - self.CS.update(can_pub_main) + if NEW_CAN: + self.CS.update(can_pub_main) + else: + for a in messaging.drain_sock(self.logcan): + canMonoTimes.append(a.logMonoTime) + can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80])) + if self.CS.accord: + self.accord_msg.extend(can_capnp_to_can_list(a.can, [9])) + self.accord_msg = self.accord_msg[-1:] + self.CS.update(can_pub_main) # create message ret = car.CarState.new_message() @@ -115,6 +162,7 @@ class CarInterface(object): # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS + ret.cruiseState.mainOn = bool(self.CS.main_on) # TODO: button presses buttonEvents = [] @@ -178,7 +226,7 @@ class CarInterface(object): if self.CS.steer_error: errors.append('steerUnavailable') elif self.CS.steer_not_allowed: - errors.append('steerTemporarilyUnavailable') + errors.append('steerTempUnavailable') if self.CS.brake_error: errors.append('brakeUnavailable') if not self.CS.gear_shifter_valid: diff --git a/selfdrive/common/params.c b/selfdrive/common/params.c index f90b171fca..980fede3ca 100644 --- a/selfdrive/common/params.c +++ b/selfdrive/common/params.c @@ -47,14 +47,15 @@ int write_db_value(const char* params_path, const char* key, const char* value, goto cleanup; } - // Move temp into place. - result = rename(tmp_path, path); + // fsync to force persist the changes. + result = fsync(tmp_fd); if (result < 0) { goto cleanup; } - // fsync to force persist the changes. - result = fsync(tmp_fd); + // Move temp into place. + result = rename(tmp_path, path); + cleanup: // Release lock. if (lock_fd >= 0) { diff --git a/selfdrive/common/timing.h b/selfdrive/common/timing.h index fab61c2094..02e934dcb5 100644 --- a/selfdrive/common/timing.h +++ b/selfdrive/common/timing.h @@ -4,6 +4,10 @@ #include #include +#ifdef __APPLE__ +#define CLOCK_BOOTTIME CLOCK_REALTIME +#endif + static inline uint64_t nanos_since_boot() { struct timespec t; clock_gettime(CLOCK_BOOTTIME, &t); diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c index dbb7bba33a..e5ab4d0a6c 100644 --- a/selfdrive/common/util.c +++ b/selfdrive/common/util.c @@ -12,13 +12,16 @@ void* read_file(const char* path, size_t* out_len) { long f_len = ftell(f); rewind(f); - char* buf = malloc(f_len + 1); + char* buf = calloc(f_len + 1, 1); assert(buf); - memset(buf, 0, f_len + 1); + size_t num_read = fread(buf, f_len, 1, f); - assert(num_read == 1); fclose(f); + if (num_read != 1) { + return NULL; + } + if (out_len) { *out_len = f_len + 1; } diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 0089f1bf3b..9b5da8e726 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -19,7 +19,10 @@ #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) +// Reads a file into a newly allocated buffer. +// // Returns NULL on failure, otherwise the NULL-terminated file contents. +// The result must be freed by the caller. void* read_file(const char* path, size_t* out_len); diff --git a/selfdrive/common/utilpp.h b/selfdrive/common/utilpp.h index a37f9fdc08..25abac6682 100644 --- a/selfdrive/common/utilpp.h +++ b/selfdrive/common/utilpp.h @@ -5,6 +5,7 @@ #include #include +#include #include #include diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 2caf37b170..338852cced 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define OPENPILOT_VERSION "0.3.3" +#define OPENPILOT_VERSION "0.3.4" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5c2946f099..f4f6cdc2f1 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,19 +2,22 @@ import os import zmq import selfdrive.messaging as messaging +from copy import copy from cereal import car, log -from selfdrive.swaglog import cloudlog from common.numpy_fast import clip from common.fingerprints import fingerprint -from selfdrive.config import Conversions as CV -from selfdrive.services import service_list from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.profiler import Profiler from common.params import Params +from selfdrive.swaglog import cloudlog +from selfdrive.config import Conversions as CV +from selfdrive.services import service_list +from selfdrive.car import get_car +from selfdrive.controls.lib.planner import Planner from selfdrive.controls.lib.drive_helpers import learn_angle_offset from selfdrive.controls.lib.longcontrol import LongControl @@ -27,321 +30,352 @@ V_CRUISE_MIN = 8 V_CRUISE_DELTA = 8 V_CRUISE_ENABLE_MIN = 40 -def controlsd_thread(gctx, rate=100): #rate in Hz - # *** log *** - context = zmq.Context() - live100 = messaging.pub_sock(context, service_list['live100'].port) - carstate = messaging.pub_sock(context, service_list['carState'].port) - carcontrol = messaging.pub_sock(context, service_list['carControl'].port) - - thermal = messaging.sub_sock(context, service_list['thermal'].port) - health = messaging.sub_sock(context, service_list['health'].port) - plan_sock = messaging.sub_sock(context, service_list['plan'].port) - - logcan = messaging.sub_sock(context, service_list['can'].port) - - # connects to can - CP = fingerprint(logcan) - - # import the car from the fingerprint - cloudlog.info("controlsd is importing %s", CP.carName) - exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface') - - sendcan = messaging.pub_sock(context, service_list['sendcan'].port) - CI = CarInterface(CP, logcan, sendcan) - - # write CarParams - params = Params() - params.put("CarParams", CP.to_bytes()) - - AM = AlertManager() - - LoC = LongControl() - LaC = LatControl() - - # fake plan - plan = log.Plan.new_message() - plan.lateralValid = False - plan.longitudinalValid = False - last_plan_time = 0 - - # controls enabled state - enabled = False - last_enable_request = 0 - - # learned angle offset - angle_offset = 0 - - # rear view camera state - rear_view_toggle = False - rear_view_allowed = bool(params.get("IsRearViewMirror")) - - v_cruise_kph = 255 +AWARENESS_TIME = 360. # 6 minutes limit without user touching steering wheels +AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car +AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted + +# class Cal +class Calibration: + UNCALIBRATED = 0 + CALIBRATED = 1 + INVALID = 2 + +# to be used +class State(): + DISABLED = 0 + ENABLED = 1 + SOFT_DISABLE = 2 + +class Controls(object): + def __init__(self, gctx, rate=100): + self.rate = rate + + # *** log *** + context = zmq.Context() + + # pub + self.live100 = messaging.pub_sock(context, service_list['live100'].port) + self.carstate = messaging.pub_sock(context, service_list['carState'].port) + self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port) + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + + # sub + self.thermal = messaging.sub_sock(context, service_list['thermal'].port) + self.health = messaging.sub_sock(context, service_list['health'].port) + logcan = messaging.sub_sock(context, service_list['can'].port) + self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port) + + self.CC = car.CarControl.new_message() + self.CI, self.CP = get_car(logcan, sendcan) + self.PL = Planner(self.CP) + self.AM = AlertManager() + self.LoC = LongControl() + self.LaC = LatControl() + + # write CarParams + params = Params() + params.put("CarParams", self.CP.to_bytes()) + + # fake plan + self.plan_ts = 0 + self.plan = log.Plan.new_message() + self.plan.lateralValid = False + self.plan.longitudinalValid = False + + # controls enabled state + self.enabled = False + self.last_enable_request = 0 + + # learned angle offset + self.angle_offset = 0 + + # rear view camera state + self.rear_view_toggle = False + self.rear_view_allowed = bool(params.get("IsRearViewMirror")) + + self.v_cruise_kph = 255 + + # 0.0 - 1.0 + self.awareness_status = 1.0 + + self.soft_disable_timer = None + + self.overtemp = False + self.free_space = 1.0 + self.cal_status = Calibration.UNCALIBRATED + self.cal_perc = 0 + + self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000) + + def data_sample(self): + self.prof = Profiler() + self.cur_time = sec_since_boot() + # first read can and compute car states + self.CS = self.CI.update() + + self.prof.checkpoint("CarInterface") - # 0.0 - 1.0 - awareness_status = 0.0 - - soft_disable_timer = None - - # Is cpu temp too high to enable? - overtemp = False - free_space = 1.0 - - # start the loop - set_realtime_priority(2) - - rk = Ratekeeper(rate, print_delay_threshold=2./1000) - while 1: - cur_time = sec_since_boot() - prof = Profiler() - - # read CAN - CS = CI.update() - - prof.checkpoint("CarInterface") + # *** thermal checking logic *** + # thermal data, checked every second + td = messaging.recv_sock(self.thermal) + if td is not None: + # Check temperature. + self.overtemp = any( + t > 950 + for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, + td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) + # under 15% of space free + self.free_space = td.thermal.freeSpace - # broadcast carState - cs_send = messaging.new_message() - cs_send.init('carState') - cs_send.carState = CS # copy? - carstate.send(cs_send.to_bytes()) + # read calibration status + cal = messaging.recv_sock(self.cal) + if cal is not None: + self.cal_status = cal.liveCalibration.calStatus + self.cal_perc = cal.liveCalibration.calPerc + - prof.checkpoint("CarState") + def state_transition(self): + pass # for now + def state_control(self): + # did it request to enable? enable_request, enable_condition = False, False - if enabled: - # gives the user 6 minutes - awareness_status -= 1.0/(100*60*6) - if awareness_status <= 0.: - AM.add("driverDistracted", enabled) - # reset awareness status on steering - if CS.steeringPressed: - awareness_status = 1.0 + if self.CS.steeringPressed or not self.enabled: + self.awareness_status = 1.0 + elif self.enabled: + # gives the user 6 minutes + self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME) + if self.awareness_status <= 0.: + self.AM.add("driverDistracted", self.enabled) + elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ + self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME: + self.AM.add("preDriverDistracted", self.enabled) # handle button presses - for b in CS.buttonEvents: + for b in self.CS.buttonEvents: print b - # reset awareness on any user action - awareness_status = 1.0 - # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": - if b.pressed and rear_view_allowed: - rear_view_toggle = True + if b.pressed and self.rear_view_allowed: + self.rear_view_toggle = True else: - rear_view_toggle = False + self.rear_view_toggle = False if b.type == "altButton1" and b.pressed: - rear_view_toggle = not rear_view_toggle + self.rear_view_toggle = not self.rear_view_toggle - if not CP.enableCruise and enabled and not b.pressed: + if not self.CP.enableCruise and self.enabled and not b.pressed: if b.type == "accelCruise": - v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA + self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA elif b.type == "decelCruise": - v_cruise_kph = v_cruise_kph - (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA - v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) + self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA + self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) - if not enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed: + if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed: enable_request = True # do disable on button down if b.type == "cancel" and b.pressed: - AM.add("disable", enabled) - - prof.checkpoint("Buttons") + self.AM.add("disable", self.enabled) + self.prof.checkpoint("Buttons") + # *** health checking logic *** - hh = messaging.recv_sock(health) + hh = messaging.recv_sock(self.health) if hh is not None: # if the board isn't allowing controls but somehow we are enabled! - if not hh.health.controlsAllowed and enabled: - AM.add("controlsMismatch", enabled) - - # *** thermal checking logic *** - - # thermal data, checked every second - td = messaging.recv_sock(thermal) - if td is not None: - # Check temperature. - overtemp = any( - t > 950 - for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, - td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) - # under 15% of space free - free_space = td.thermal.freeSpace - - prof.checkpoint("Health") + # TODO: this should be in state transition with a function follower logic + if not hh.health.controlsAllowed and self.enabled: + self.AM.add("controlsMismatch", self.enabled) # disable if the pedals are pressed while engaged, this is a user disable - if enabled: - if CS.gasPressed or CS.brakePressed: - AM.add("disable", enabled) + if self.enabled: + if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.mainOn: + self.AM.add("disable", self.enabled) - # how did we get into this state? - if CP.enableCruise and not CS.cruiseState.enabled: - AM.add("cruiseDisabled", enabled) + # it can happen that car cruise disables while comma system is enabled: need to + # keep braking if needed or if the speed is very low + # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert + if self.CP.enableCruise and not self.CS.cruiseState.enabled and \ + (self.CC.brake <= 0. or self.CS.vEgo < 0.3): + self.AM.add("cruiseDisabled", self.enabled) if enable_request: # check for pressed pedals - if CS.gasPressed or CS.brakePressed: - AM.add("pedalPressed", enabled) + if self.CS.gasPressed or self.CS.brakePressed: + self.AM.add("pedalPressed", self.enabled) enable_request = False else: - print "enabled pressed at", cur_time - last_enable_request = cur_time + print "enabled pressed at", self.cur_time + self.last_enable_request = self.cur_time # don't engage with less than 15% free - if free_space < 0.15: - AM.add("outOfSpace", enabled) + if self.free_space < 0.15: + self.AM.add("outOfSpace", self.enabled) enable_request = False - if CP.enableCruise: - enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled + if self.CP.enableCruise: + enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled else: enable_condition = enable_request - if CP.enableCruise and CS.cruiseState.enabled: - v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + if self.CP.enableCruise and self.CS.cruiseState.enabled: + self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH - prof.checkpoint("AdaptiveCruise") + self.prof.checkpoint("AdaptiveCruise") # *** what's the plan *** - new_plan = messaging.recv_sock(plan_sock) - if new_plan is not None: - plan = new_plan.plan - plan = plan.as_builder() # plan can change in controls - last_plan_time = cur_time - - # check plan for timeout - if cur_time - last_plan_time > 0.5: - plan.lateralValid = False - plan.longitudinalValid = False - - # gives 18 seconds before decel begins (w 6 minute timeout) - if awareness_status < -0.05: - plan.aTargetMax = min(plan.aTargetMax, -0.2) - plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax) - - if enable_request or enable_condition or enabled: + plan_packet = self.PL.update(self.CS, self.LoC) + self.plan = plan_packet.plan + self.plan_ts = plan_packet.logMonoTime + + # if user is not responsive to awareness alerts, then start a smooth deceleration + if self.awareness_status < -0.: + self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL) + self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax) + + if enable_request or enable_condition or self.enabled: # add all alerts from car - for alert in CS.errors: - AM.add(alert, enabled) + for alert in self.CS.errors: + self.AM.add(alert, self.enabled) - if not plan.longitudinalValid: - AM.add("radarCommIssue", enabled) + if not self.plan.longitudinalValid: + self.AM.add("radarCommIssue", self.enabled) - if not plan.lateralValid: + if self.cal_status != Calibration.CALIBRATED: + if self.cal_status == Calibration.UNCALIBRATED: + self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%') + else: + self.AM.add("calibrationInvalid", self.enabled) + + if not self.plan.lateralValid: # If the model is not broadcasting, assume that it is because # the user has uploaded insufficient data for calibration. # Other cases that would trigger this are rare and unactionable by the user. - AM.add("dataNeeded", enabled) - - if overtemp: - AM.add("overheat", enabled) - - # *** angle offset learning *** - if rk.frame % 5 == 2 and plan.lateralValid: - # *** run this at 20hz again *** - angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, plan.dPoly, LaC.y_des, CS.steeringPressed) - - # *** gas/brake PID loop *** - final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph, - plan.vTarget, - [plan.aTargetMin, plan.aTargetMax], - plan.jerkFactor, CP) + self.AM.add("dataNeeded", self.enabled) + + if self.overtemp: + self.AM.add("overheat", self.enabled) + + + # *** angle offset learning *** + if self.rk.frame % 5 == 2 and self.plan.lateralValid: + # *** run this at 20hz again *** + self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, + self.plan.dPoly, self.LaC.y_des, self.CS.steeringPressed) + + # *** gas/brake PID loop *** + final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, + self.plan.vTarget, + [self.plan.aTargetMin, self.plan.aTargetMax], + self.plan.jerkFactor, self.CP) + + # *** steering PID loop *** + final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle, + self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.CP) + + self.prof.checkpoint("PID") + + # ***** handle alerts **** + # send FCW alert if triggered by planner + if self.plan.fcw: + self.AM.add("fcw", self.enabled) - # *** steering PID loop *** - final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly, angle_offset, CP) - - prof.checkpoint("PID") - - # ***** handle alerts **** # send a "steering required alert" if saturation count has reached the limit if sat_flag: - AM.add("steerSaturated", enabled) + self.AM.add("steerSaturated", self.enabled) - if enabled and AM.alertShouldDisable(): + if self.enabled and self.AM.alertShouldDisable(): print "DISABLING IMMEDIATELY ON ALERT" - enabled = False + self.enabled = False - if enabled and AM.alertShouldSoftDisable(): - if soft_disable_timer is None: - soft_disable_timer = 3 * rate - elif soft_disable_timer == 0: + if self.enabled and self.AM.alertShouldSoftDisable(): + if self.soft_disable_timer is None: + self.soft_disable_timer = 3 * self.rate + elif self.soft_disable_timer == 0: print "SOFT DISABLING ON ALERT" - enabled = False + self.enabled = False else: - soft_disable_timer -= 1 + self.soft_disable_timer -= 1 else: - soft_disable_timer = None + self.soft_disable_timer = None - if enable_condition and not enabled and not AM.alertPresent(): + if enable_condition and not self.enabled and not self.AM.alertPresent(): print "*** enabling controls" # beep for enabling - AM.add("enable", enabled) + self.AM.add("enable", self.enabled) # enable both lateral and longitudinal controls - enabled = True + self.enabled = True # on activation, let's always set v_cruise from where we are, even if PCM ACC is active - v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) + self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) # 6 minutes driver you're on - awareness_status = 1.0 + self.awareness_status = 1.0 # reset the PID loops - LaC.reset() + self.LaC.reset() # start long control at actual speed - LoC.reset(v_pid = CS.vEgo) + self.LoC.reset(v_pid = self.CS.vEgo) # *** push the alerts to current *** - alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts(cur_time) - + # TODO: remove output, store them inside AM class instead + self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time) + # ***** control the car ***** - CC = car.CarControl.new_message() + self.CC.enabled = self.enabled - CC.enabled = enabled + self.CC.gas = float(final_gas) + self.CC.brake = float(final_brake) + self.CC.steeringTorque = float(final_steer) - CC.gas = float(final_gas) - CC.brake = float(final_brake) - CC.steeringTorque = float(final_steer) - - CC.cruiseControl.override = True - CC.cruiseControl.cancel = bool((not CP.enableCruise) or (not enabled and CS.cruiseState.enabled)) # always cancel if we have an interceptor + self.CC.cruiseControl.override = True + # always cancel if we have an interceptor + self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or + (not self.enabled and self.CS.cruiseState.enabled)) # brake discount removes a sharp nonlinearity brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0)) - CC.cruiseControl.speedOverride = float(max(0.0, ((LoC.v_pid - .5) * brake_discount)) if CP.enableCruise else 0.0) + self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0) #CC.cruiseControl.accelOverride = float(AC.a_pcm) # TODO: fix this - CC.cruiseControl.accelOverride = float(1.0) - - CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) - CC.hudControl.speedVisible = enabled - CC.hudControl.lanesVisible = enabled - CC.hudControl.leadVisible = plan.hasLead + self.CC.cruiseControl.accelOverride = float(1.0) - CC.hudControl.visualAlert = visual_alert - CC.hudControl.audibleAlert = audible_alert + self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) + self.CC.hudControl.speedVisible = self.enabled + self.CC.hudControl.lanesVisible = self.enabled + self.CC.hudControl.leadVisible = self.plan.hasLead + self.CC.hudControl.visualAlert = self.visual_alert + self.CC.hudControl.audibleAlert = self.audible_alert + # TODO: remove it from here and put it in state_transition # this alert will apply next controls cycle - if not CI.apply(CC): - AM.add("controlsFailed", enabled) + if not self.CI.apply(self.CC): + self.AM.add("controlsFailed", self.enabled) - # broadcast carControl + def data_send(self): + + # broadcast carControl first cc_send = messaging.new_message() cc_send.init('carControl') - cc_send.carControl = CC # copy? - carcontrol.send(cc_send.to_bytes()) + cc_send.carControl = copy(self.CC) + self.carcontrol.send(cc_send.to_bytes()) - prof.checkpoint("CarControl") + self.prof.checkpoint("CarControl") + # broadcast carState + cs_send = messaging.new_message() + cs_send.init('carState') + cs_send.carState = copy(self.CS) + self.carstate.send(cs_send.to_bytes()) + # ***** publish state to logger ***** # publish controls state at 100Hz @@ -349,56 +383,72 @@ def controlsd_thread(gctx, rate=100): #rate in Hz dat.init('live100') # show rear view camera on phone if in reverse gear or when button is pressed - dat.live100.rearViewCam = ('reverseGear' in CS.errors and rear_view_allowed) or rear_view_toggle - dat.live100.alertText1 = alert_text_1 - dat.live100.alertText2 = alert_text_2 - dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0 + dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle + dat.live100.alertText1 = self.alert_text_1 + dat.live100.alertText2 = self.alert_text_2 + dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0 # what packets were used to process - dat.live100.canMonoTimes = list(CS.canMonoTimes) + dat.live100.canMonoTimes = list(self.CS.canMonoTimes) + dat.live100.planMonoTime = self.plan_ts # if controls is enabled - dat.live100.enabled = enabled + dat.live100.enabled = self.enabled # car state - dat.live100.vEgo = CS.vEgo - dat.live100.angleSteers = CS.steeringAngle - dat.live100.steerOverride = CS.steeringPressed + dat.live100.vEgo = self.CS.vEgo + dat.live100.angleSteers = self.CS.steeringAngle + dat.live100.steerOverride = self.CS.steeringPressed # longitudinal control state - dat.live100.vPid = float(LoC.v_pid) - dat.live100.vCruise = float(v_cruise_kph) - dat.live100.upAccelCmd = float(LoC.Up_accel_cmd) - dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd) + dat.live100.vPid = float(self.LoC.v_pid) + dat.live100.vCruise = float(self.v_cruise_kph) + dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd) + dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd) # lateral control state - dat.live100.yActual = float(LaC.y_actual) - dat.live100.yDes = float(LaC.y_des) - dat.live100.upSteer = float(LaC.Up_steer) - dat.live100.uiSteer = float(LaC.Ui_steer) + dat.live100.yActual = float(self.LaC.y_actual) + dat.live100.yDes = float(self.LaC.y_des) + dat.live100.upSteer = float(self.LaC.Up_steer) + dat.live100.uiSteer = float(self.LaC.Ui_steer) # processed radar state, should add a_pcm? - dat.live100.vTargetLead = float(plan.vTarget) - dat.live100.aTargetMin = float(plan.aTargetMin) - dat.live100.aTargetMax = float(plan.aTargetMax) - dat.live100.jerkFactor = float(plan.jerkFactor) + dat.live100.vTargetLead = float(self.plan.vTarget) + dat.live100.aTargetMin = float(self.plan.aTargetMin) + dat.live100.aTargetMax = float(self.plan.aTargetMax) + dat.live100.jerkFactor = float(self.plan.jerkFactor) # log learned angle offset - dat.live100.angleOffset = float(angle_offset) + dat.live100.angleOffset = float(self.angle_offset) # lag - dat.live100.cumLagMs = -rk.remaining*1000. + dat.live100.cumLagMs = -self.rk.remaining*1000. - live100.send(dat.to_bytes()) + self.live100.send(dat.to_bytes()) - prof.checkpoint("Live100") + self.prof.checkpoint("Live100") + def wait(self): # *** run loop at fixed rate *** - if rk.keep_time(): - prof.display() + if self.rk.keep_time(): + self.prof.display() + + +def controlsd_thread(gctx, rate=100): + # start the loop + set_realtime_priority(2) + CTRLS = Controls(gctx, rate) + while 1: + CTRLS.data_sample() + CTRLS.state_transition() + CTRLS.state_control() + CTRLS.data_send() + CTRLS.wait() + def main(gctx=None): controlsd_thread(gctx, 100) if __name__ == "__main__": main() + diff --git a/selfdrive/controls/lib/adaptivecruise.py b/selfdrive/controls/lib/adaptivecruise.py index f7a42c8cc9..3b3207e7c6 100644 --- a/selfdrive/controls/lib/adaptivecruise.py +++ b/selfdrive/controls/lib/adaptivecruise.py @@ -214,28 +214,6 @@ def calc_jerk_factor(d_lead, v_rel): return jerk_factor -def calc_ttc(d_rel, v_rel, a_rel, v_lead): - # this function returns the time to collision (ttc), assuming that a_rel will stay constant - # TODO: Review these assumptions. - # change sign to rel quantities as it's going to be easier for calculations - v_rel = -v_rel - a_rel = -a_rel - - # assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel - # this helps overweighting a_rel when v_lead is close to zero. - t_decel = 2. - a_rel = min(a_rel, v_lead/t_decel) - - delta = v_rel**2 + 2 * d_rel * a_rel - # assign an arbitrary high ttc value if there is no solution to ttc - if delta < 0.1: - ttc = 5. - elif math.sqrt(delta) + v_rel < 0.1: - ttc = 5. - else: - ttc = 2 * d_rel / (math.sqrt(delta) + v_rel) - return ttc - MAX_SPEED_POSSIBLE = 55. @@ -299,18 +277,14 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP): class AdaptiveCruise(object): - def __init__(self, live20): - self.live20 = live20 + def __init__(self): self.last_cal = 0. self.l1, self.l2 = None, None - self.logMonoTime = 0 self.dead = True - def update(self, cur_time, v_ego, angle_steers, v_pid, CP): - l20 = messaging.recv_sock(self.live20) + def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20): if l20 is not None: self.l1 = l20.live20.leadOne self.l2 = l20.live20.leadTwo - self.logMonoTime = l20.logMonoTime # TODO: no longer has anything to do with calibration self.last_cal = cur_time diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 1cbdbe1657..162a1977ed 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -1,13 +1,14 @@ from cereal import car from selfdrive.swaglog import cloudlog +import copy class ET: ENABLE = 0 NO_ENTRY = 1 WARNING = 2 - SOFT_DISABLE = 3 - IMMEDIATE_DISABLE = 4 - USER_DISABLE = 5 + USER_DISABLE = 3 + SOFT_DISABLE = 4 + IMMEDIATE_DISABLE = 5 class alert(object): def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text): @@ -34,33 +35,37 @@ class alert(object): class AlertManager(object): alerts = { - "enable": alert("", "", ET.ENABLE, None, "beepSingle", .2, 0., 0.), - "disable": alert("", "", ET.USER_DISABLE, None, "beepSingle", .2, 0., 0.), - "pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.), - "driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", 1., 1., 1.), - "steerSaturated": alert("Take Control", "Steering Limit Reached", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.), - "overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "radarCommIssue": alert("Take Control Immediately", "Radar Comm Issue", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "modelCommIssue": alert("Take Control Immediately", "Model Comm Issue", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "controlsFailed": alert("Take Control Immediately", "Controls Failed", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "enable": alert("", "", ET.ENABLE, None, "beepSingle", .2, 0., 0.), + "disable": alert("", "", ET.USER_DISABLE, None, "beepSingle", .2, 0., 0.), + "pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.), + "preDriverDistracted": alert("Take Control ", "User Distracted", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.), + "driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", .5, .5, .5), + "steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.), + "overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "radarCommIssue": alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "calibrationInvalid": alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "modelCommIssue": alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "controlsFailed": alert("Take Control Immediately", "Controls Failed", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "fcw": alert("", "", ET.WARNING, None, None, .1, .1, .1), # car errors - "commIssue": alert("Take Control Immediately","Communication Issues", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "steerUnavailable": alert("Take Control Immediately","Steer Error", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "steerTemporarilyUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeRepeated", 1., 2., 3.), - "brakeUnavailable": alert("Take Control Immediately","Brake Error", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "gasUnavailable": alert("Take Control Immediately","Gas Error", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "wrongGear": alert("Take Control Immediately","Gear not D", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "reverseGear": alert("Take Control Immediately","Car in Reverse", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "doorOpen": alert("Take Control Immediately","Door Open", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "espDisabled": alert("Take Control Immediately","ESP Off", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "cruiseDisabled": alert("Take Control Immediately","Cruise Is Off", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), - "outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), - "dataNeeded": alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), - "ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.), + "commIssue": alert("Take Control Immediately","CAN Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "steerTempUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.), + "brakeUnavailable": alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "gasUnavailable": alert("Take Control Immediately","Gas Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "wrongGear": alert("Take Control Immediately","Gear not D", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "reverseGear": alert("Take Control Immediately","Car in Reverse", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "doorOpen": alert("Take Control Immediately","Door Open", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "espDisabled": alert("Take Control Immediately","ESP Off", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "cruiseDisabled": alert("Take Control Immediately","Cruise Is Off", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), + "outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), + "dataNeeded": alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), + "ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), + "startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.), } def __init__(self): self.activealerts = [] @@ -71,25 +76,27 @@ class AlertManager(object): return len(self.activealerts) > 0 def alertShouldSoftDisable(self): - return len(self.activealerts) > 0 and self.activealerts[0].alert_type == ET.SOFT_DISABLE + return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts) def alertShouldDisable(self): - return len(self.activealerts) > 0 and self.activealerts[0].alert_type >= ET.IMMEDIATE_DISABLE + return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts) - def add(self, alert_type, enabled = True): + def add(self, alert_type, enabled=True, extra_text=''): alert_type = str(alert_type) - this_alert = self.alerts[alert_type] - - # downgrade the alert if we aren't enabled - if not enabled and this_alert.alert_type > ET.NO_ENTRY: + this_alert = copy.copy(self.alerts[alert_type]) + this_alert.alert_text_2 += extra_text + # downgrade the alert if we aren't enabled, except if it's FCW, which remains the same + # TODO: remove this 'if' by adding more alerts + if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \ + and this_alert != self.alerts['fcw']: this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.) # ignore no entries if we are enabled - if enabled and this_alert.alert_type < ET.WARNING: + if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]: return - # if new alert is different, log it - if self.current_alert is None or self.current_alert.alert_text_2 != this_alert.alert_text_2: + # if new alert is higher priority, log it + if self.current_alert is None or this_alert > self.current_alert: cloudlog.event('alert_add', alert_type=alert_type, enabled=enabled) @@ -102,7 +109,6 @@ class AlertManager(object): self.alert_start_time = cur_time self.current_alert = self.activealerts[0] print self.current_alert - alert_text_1 = "" alert_text_2 = "" visual_alert = "none" diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py new file mode 100644 index 0000000000..c9d6507057 --- /dev/null +++ b/selfdrive/controls/lib/fcw.py @@ -0,0 +1,66 @@ +import numpy as np +from common.realtime import sec_since_boot + +#Time to collisions greater than 5s are iognored +MAX_TTC = 5. + +def calc_ttc(l1): + # if l1 is None, return max ttc immediately + if not l1: + return MAX_TTC + # this function returns the time to collision (ttc), assuming that + # ARel will stay constant TODO: review this assumptions + # change sign to rel quantities as it's going to be easier for calculations + vRel = -l1.vRel + aRel = -l1.aRel + + # assuming that closing gap ARel comes from lead vehicle decel, + # then limit ARel so that v_lead will get to zero in no sooner than t_decel. + # This helps underweighting ARel when v_lead is close to zero. + t_decel = 2. + aRel = np.minimum(aRel, l1.vLead/t_decel) + + # delta of the quadratic equation to solve for ttc + delta = vRel**2 + 2 * l1.dRel * aRel + + # assign an arbitrary high ttc value if there is no solution to ttc + if delta < 0.1 or (np.sqrt(delta) + vRel < 0.1): + ttc = MAX_TTC + else: + ttc = np.minimum(2 * l1.dRel / (np.sqrt(delta) + vRel), MAX_TTC) + return ttc + +class ForwardCollisionWarning(object): + def __init__(self, dt): + self.last_active = 0. + self.violation_time = 0. + self.active = False + self.dt = dt # time step + + def process(self, CS, AC): + # send an fcw alert if the violation time > violation_thrs + violation_thrs = 0.3 # fcw turns on after a continuous violation for this time + fcw_t_delta = 5. # no more than one fcw alert within this time + a_acc_on = -2.0 # with system on, above this limit of desired decel, we should trigger fcw + a_acc_off = -2.5 # with system off, above this limit of desired decel, we should trigger fcw + ttc_thrs = 2.5 # ttc threshold for fcw + v_fcw_min = 2. # no fcw below 2m/s + steer_angle_th = 40. # deg, no fcw above this steer angle + cur_time = sec_since_boot() + + ttc = calc_ttc(AC.l1) + a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off + + # increase violation time if we want to decelerate quite fast + if AC.l1 and ( \ + (CS.vEgo > v_fcw_min) and (CS.vEgo > AC.v_target_lead) and (AC.a_target[0] < a_fcw) \ + and not CS.brakePressed and ttc < ttc_thrs and abs(CS.steeringAngle) < steer_angle_th \ + and AC.l1.fcw): + self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs) + else: + self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0) + + # fire FCW + self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta) + if self.active: + self.last_active = cur_time diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d89c976dde..4c217cab03 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,6 +1,6 @@ import math import numpy as np -from common.numpy_fast import clip +from common.numpy_fast import clip, interp def calc_curvature(v_ego, angle_steers, CP, angle_offset=0): deg_to_rad = np.pi/180. @@ -8,15 +8,28 @@ def calc_curvature(v_ego, angle_steers, CP, angle_offset=0): curvature = angle_steers_rad/(CP.steerRatio * CP.wheelBase * (1. + CP.slipFactor * v_ego**2)) return curvature -def calc_d_lookahead(v_ego): +_K_CURV_V = [1., 0.6] +_K_CURV_BP = [0., 0.002] + +def calc_d_lookahead(v_ego, d_poly): #*** this function computes how far too look for lateral control - # howfar we look ahead is function of speed + # howfar we look ahead is function of speed and how much curvy is the path offset_lookahead = 1. - coeff_lookahead = 4.4 + k_lookahead = 7. + # integrate abs value of second derivative of poly to get a measure of path curvature + pts_len = 50. # m + if len(d_poly)>0: + pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len)) + else: + pts = 0. + curv = np.sum(np.abs(pts))/pts_len + + k_curv = interp(curv, _K_CURV_BP, _K_CURV_V) + # sqrt on speed is needed to keep, for a given curvature, the y_offset # proportional to speed. Indeed, y_offset is prop to d_lookahead^2 - # 26m at 25m/s - d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead + # 36m at 25m/s + d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv return d_lookahead def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, CP, angle_offset): @@ -101,8 +114,8 @@ class LatControl(object): steer_max = 1.0 - # how far we look ahead is function of speed - d_lookahead = calc_d_lookahead(v_ego) + # how far we look ahead is function of speed and desired path + d_lookahead = calc_d_lookahead(v_ego, d_poly) # calculate actual offset at the lookahead point self.y_actual, _ = calc_lookahead_offset(v_ego, angle_steers, diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index bcb92e126e..d7b0e6a135 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -39,21 +39,35 @@ def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed): d_poly = list((c_poly*c_prob + p_poly*p_prob*p_weight ) / (c_prob + p_prob*p_weight)) return d_poly, c_poly, c_prob -class PathPlanner(object): +class OptPathPlanner(object): def __init__(self, model): self.model = model self.dead = True self.d_poly = [0., 0., 0., 0.] self.last_model = 0. - self.logMonoTime = 0 - self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() - def update(self, cur_time, v_ego): - md = messaging.recv_sock(self.model) + def update(self, cur_time, v_ego, md): + if md is not None: + # simple compute of the center of the lane + pts = [(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)] + self.d_poly = model_polyfit(pts, self._path_pinv) + + self.last_model = cur_time + self.dead = False + elif cur_time - self.last_model > 0.5: + self.dead = True + +class PathPlanner(object): + def __init__(self): + self.dead = True + self.d_poly = [0., 0., 0., 0.] + self.last_model = 0. + self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 + self._path_pinv = compute_path_pinv() + def update(self, cur_time, v_ego, md): if md is not None: - self.logMonoTime = md.logMonoTime p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py new file mode 100755 index 0000000000..e9e7adb671 --- /dev/null +++ b/selfdrive/controls/lib/planner.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python +import os +import zmq +import numpy as np +import selfdrive.messaging as messaging + +from selfdrive.services import service_list +from common.realtime import sec_since_boot +from common.params import Params + +from selfdrive.swaglog import cloudlog +from cereal import car + +from selfdrive.controls.lib.pathplanner import OptPathPlanner +from selfdrive.controls.lib.pathplanner import PathPlanner +from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise +from selfdrive.controls.lib.fcw import ForwardCollisionWarning + +_DT = 0.01 # 100Hz + +class Planner(object): + def __init__(self, CP): + context = zmq.Context() + self.CP = CP + self.live20 = messaging.sub_sock(context, service_list['live20'].port) + self.model = messaging.sub_sock(context, service_list['model'].port) + + self.plan = messaging.pub_sock(context, service_list['plan'].port) + + self.last_md_ts = 0 + self.last_l20_ts = 0 + + if os.getenv("OPT") is not None: + self.PP = OptPathPlanner(self.model) + else: + self.PP = PathPlanner() + self.AC = AdaptiveCruise() + self.FCW = ForwardCollisionWarning(_DT) + + # this runs whenever we get a packet that can change the plan + + def update(self, CS, LoC): + cur_time = sec_since_boot() + + md = messaging.recv_sock(self.model) + if md is not None: + self.last_md_ts = md.logMonoTime + l20 = messaging.recv_sock(self.live20) + if l20 is not None: + self.last_l20_ts = l20.logMonoTime + + self.PP.update(cur_time, CS.vEgo, md) + + # LoC.v_pid -> CS.vEgo + # TODO: is this change okay? + self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) + + # **** send the plan **** + plan_send = messaging.new_message() + plan_send.init('plan') + + plan_send.plan.mdMonoTime = self.last_md_ts + plan_send.plan.l20MonoTime = self.last_l20_ts + + # lateral plan + plan_send.plan.lateralValid = not self.PP.dead + if plan_send.plan.lateralValid: + plan_send.plan.dPoly = map(float, self.PP.d_poly) + + # longitudal plan + plan_send.plan.longitudinalValid = not self.AC.dead + if plan_send.plan.longitudinalValid: + plan_send.plan.vTarget = float(self.AC.v_target_lead) + plan_send.plan.aTargetMin = float(self.AC.a_target[0]) + plan_send.plan.aTargetMax = float(self.AC.a_target[1]) + plan_send.plan.jerkFactor = float(self.AC.jerk_factor) + plan_send.plan.hasLead = self.AC.has_lead + + # compute risk of collision events: fcw + self.FCW.process(CS, self.AC) + plan_send.plan.fcw = bool(self.FCW.active) + + self.plan.send(plan_send.to_bytes()) + return plan_send diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 9d2216ccf4..370534c40e 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -205,7 +205,7 @@ class Cluster(object): lead.vLeadK = float(self.vLeadK) lead.aLeadK = float(self.aLeadK) lead.status = True - lead.fcw = False + lead.fcw = self.is_potential_fcw() def __str__(self): ret = "x: %7.2f y: %7.2f v: %7.2f a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel) @@ -240,18 +240,18 @@ class Cluster(object): d_path = max(d_path + lat_corr, 0) - if d_path < 1.5 and not self.stationary and not self.oncoming: - return True - else: - return False + return d_path < 1.5 and not self.stationary and not self.oncoming def is_potential_lead2(self, lead_clusters): if len(lead_clusters) > 0: lead_cluster = lead_clusters[0] - # check if the new lead is too close and roughly at the same speed of the first lead: it might just be the second axle of the same vehicle - if (self.dRel - lead_cluster.dRel) < 8. and abs(self.vRel - lead_cluster.vRel) < 1.: - return False - else: - return True + # check if the new lead is too close and roughly at the same speed of the first lead: + # it might just be the second axle of the same vehicle + return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1. else: return False + + def is_potential_fcw(self): + # is this cluster trustrable enough for triggering fcw? + # fcw can trigger only on clusters that have been fused vision model for at least 20 frames + return self.vision_cnt >= 20 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py deleted file mode 100755 index 692a770636..0000000000 --- a/selfdrive/controls/plannerd.py +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/env python -import os -import zmq -import numpy as np -import selfdrive.messaging as messaging - -from selfdrive.services import service_list -from common.realtime import sec_since_boot, set_realtime_priority -from common.params import Params - -from selfdrive.swaglog import cloudlog -from cereal import car - -from selfdrive.controls.lib.pathplanner import PathPlanner -from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise - -def plannerd_thread(gctx): - context = zmq.Context() - poller = zmq.Poller() - - carstate = messaging.sub_sock(context, service_list['carState'].port, poller) - live20 = messaging.sub_sock(context, service_list['live20'].port) - model = messaging.sub_sock(context, service_list['model'].port) - - plan = messaging.pub_sock(context, service_list['plan'].port) - - # wait for stats about the car to come in from controls - cloudlog.info("plannerd is waiting for CarParams") - CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) - cloudlog.info("plannerd got CarParams") - - CS = None - PP = PathPlanner(model) - AC = AdaptiveCruise(live20) - - # start the loop - set_realtime_priority(2) - - # this runs whenever we get a packet that can change the plan - while True: - polld = poller.poll(timeout=1000) - for sock, mode in polld: - if mode != zmq.POLLIN or sock != carstate: - continue - - cur_time = sec_since_boot() - CS = messaging.recv_sock(carstate).carState - - PP.update(cur_time, CS.vEgo) - - # LoC.v_pid -> CS.vEgo - # TODO: is this change okay? - AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP) - - # **** send the plan **** - plan_send = messaging.new_message() - plan_send.init('plan') - - # lateral plan - plan_send.plan.lateralValid = not PP.dead - if plan_send.plan.lateralValid: - plan_send.plan.dPoly = map(float, PP.d_poly) - - # longitudal plan - plan_send.plan.longitudinalValid = not AC.dead - if plan_send.plan.longitudinalValid: - plan_send.plan.vTarget = float(AC.v_target_lead) - plan_send.plan.aTargetMin = float(AC.a_target[0]) - plan_send.plan.aTargetMax = float(AC.a_target[1]) - plan_send.plan.jerkFactor = float(AC.jerk_factor) - plan_send.plan.hasLead = AC.has_lead - - plan.send(plan_send.to_bytes()) - - -def main(gctx=None): - plannerd_thread(gctx) - -if __name__ == "__main__": - main() - diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 60885b4cb8..936b81294e 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -20,6 +20,7 @@ from common.params import Params from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.kalman.ekf import EKF, SimpleSensor +VISION_ONLY = False #vision point DIMSV = 2 @@ -62,9 +63,12 @@ def radard_thread(gctx=None): model = messaging.sub_sock(context, service_list['model'].port) live100 = messaging.sub_sock(context, service_list['live100'].port) - PP = PathPlanner(model) + PP = PathPlanner() RI = RadarInterface() + last_md_ts = 0 + last_l100_ts = 0 + # *** publish live20 and liveTracks live20 = messaging.pub_sock(context, service_list['live20'].port) liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port) @@ -109,18 +113,24 @@ def radard_thread(gctx=None): v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1) v_ego_array = v_ego_array[:, 1:] + last_l100_ts = l100.logMonoTime + if v_ego is None: continue + md = messaging.recv_sock(model) + if md is not None: + last_md_ts = md.logMonoTime + # *** get path prediction from the model *** - PP.update(sec_since_boot(), v_ego) + PP.update(sec_since_boot(), v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var)) ekfv.predict(tsv) ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), - float(ekfv.state[SPEEDV]), np.nan, PP.logMonoTime, np.nan, sec_since_boot()) + float(ekfv.state[SPEEDV]), np.nan, last_md_ts, np.nan, sec_since_boot()) else: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) @@ -142,7 +152,9 @@ def radard_thread(gctx=None): # *** compute the tracks *** for ids in ar_pts: # ignore the vision point for now - if ids == VISION_POINT: + if ids == VISION_POINT and not VISION_ONLY: + continue + elif ids != VISION_POINT and VISION_ONLY: continue rpt = ar_pts[ids] @@ -212,8 +224,9 @@ def radard_thread(gctx=None): # *** publish live20 *** dat = messaging.new_message() dat.init('live20') - dat.live20.mdMonoTime = PP.logMonoTime + dat.live20.mdMonoTime = last_md_ts dat.live20.canMonoTimes = list(rr.canMonoTimes) + dat.live20.l100MonoTime = last_l100_ts if lead_len > 0: lead_clusters[0].toLive20(dat.live20.leadOne) if lead2_len > 0: diff --git a/selfdrive/debug/test_carcontroller.py b/selfdrive/debug/test_carcontroller.py index 54fa1cfe8d..4d9dcbda86 100755 --- a/selfdrive/debug/test_carcontroller.py +++ b/selfdrive/debug/test_carcontroller.py @@ -1,17 +1,17 @@ #!/usr/bin/env python -from evdev import InputDevice -from select import select import time import numpy as np import zmq +from evdev import InputDevice +from select import select from cereal import car +from common.realtime import Ratekeeper import selfdrive.messaging as messaging from selfdrive.services import service_list -from common.realtime import Ratekeeper +from selfdrive.car import get_car -from common.fingerprints import fingerprint if __name__ == "__main__": # ***** connect to joystick ***** @@ -27,10 +27,7 @@ if __name__ == "__main__": logcan = messaging.sub_sock(context, service_list['can'].port) sendcan = messaging.pub_sock(context, service_list['sendcan'].port) - CP = fingerprint(logcan) - exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface') - - CI = CarInterface(CP, logcan, sendcan) + CI, CP = get_car(logcan, sendcan) rk = Ratekeeper(100) diff --git a/selfdrive/debug/test_carstate.py b/selfdrive/debug/test_carstate.py index 114f1446bc..f88c521d1f 100755 --- a/selfdrive/debug/test_carstate.py +++ b/selfdrive/debug/test_carstate.py @@ -5,7 +5,7 @@ import zmq import selfdrive.messaging as messaging from selfdrive.services import service_list -from common.fingerprints import fingerprint +from selfdrive.car import get_car def bpressed(CS, btype): for b in CS.buttonEvents: @@ -17,10 +17,7 @@ def test_loop(): context = zmq.Context() logcan = messaging.sub_sock(context, service_list['can'].port) - CP = fingerprint(logcan) - exec('from selfdrive.car.'+CP.carName+'.interface import CarInterface') - - CI = CarInterface(CP, logcan, None) + CI, CP = get_car(logcan) state = 0 diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 6f4bfcf91d..138c4f1847 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:11408b2637323c15dfccdb0b6059cb45f9073ac700ab1efe233f43ccb61184a7 -size 1357296 +oid sha256:706438daafdc304a4e31539dd6bd01f2ea6b349990756ff80d79728ae5a849a6 +size 1365184 diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index ff5be1e2d5..34a3a79aaa 100755 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -117,7 +117,12 @@ class Uploader(object): if name in ["rlog", "rlog.bz2"]: return (key, fn, 0) - # then upload camera files no not on wifi + # then upload compressed camera file + for name, key, fn in self.gen_upload_files(): + if name in ["fcamera.hevc"]: + return (key, fn, 1) + + # then upload other files for name, key, fn in self.gen_upload_files(): if not name.endswith('.lock') and not name.endswith(".tmp"): return (key, fn, 1) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4e183d03f5..136cd73457 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -42,7 +42,6 @@ BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") managed_processes = { "uploader": "selfdrive.loggerd.uploader", "controlsd": "selfdrive.controls.controlsd", - "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", "loggerd": ("loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", @@ -66,7 +65,6 @@ interrupt_processes = [] car_started_processes = [ 'controlsd', - 'plannerd', 'loggerd', 'sensord', 'radard', @@ -345,8 +343,8 @@ def get_installed_apks(): # optional, build the c++ binaries and preimport the python for speed def manager_prepare(): # update submodules - system("cd %s && git submodule init" % BASEDIR) - system("cd %s && git submodule update" % BASEDIR) + system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR) + system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR) # build cereal first subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) @@ -452,9 +450,20 @@ def main(): if params.get("IsRearViewMirror") is None: params.put("IsRearViewMirror", "1") - manager_init() - manager_prepare() - + # put something on screen while we set things up + if os.getenv("PREPAREONLY") is not None: + spinner_proc = None + else: + spinner_proc = subprocess.Popen(["./spinner", "loading openpilot..."], + cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), + close_fds=True) + try: + manager_init() + manager_prepare() + finally: + if spinner_proc: + spinner_proc.terminate() + if os.getenv("PREPAREONLY") is not None: sys.exit(0) diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index e1da1b3d90..554ce944c6 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -1,14 +1,21 @@ #!/usr/bin/env python +import os import numpy as np + from selfdrive.car.honda.can_parser import CANParser +from selfdrive.can.parser import CANParser as CANParserC + from selfdrive.boardd.boardd import can_capnp_to_can_list from cereal import car +from common.realtime import sec_since_boot import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging +NEW_CAN = os.getenv("OLD_CAN") is None + def _create_radard_can_parser(): dbc_f = 'acura_ilx_2016_nidec.dbc' radar_messages = range(0x430, 0x43A) + range(0x440, 0x446) @@ -17,7 +24,10 @@ def _create_radard_can_parser(): [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) checks = zip(radar_messages, [20]*16) - return CANParser(dbc_f, signals, checks) + if NEW_CAN: + return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 1) + else: + return CANParser(dbc_f, signals, checks) class RadarInterface(object): def __init__(self): @@ -33,19 +43,28 @@ class RadarInterface(object): def update(self): canMonoTimes = [] - can_pub_radar = [] - # TODO: can hang if no packets show up - while 1: - for a in messaging.drain_sock(self.logcan, wait_for_one=True): - canMonoTimes.append(a.logMonoTime) - can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) + if NEW_CAN: + updated_messages = set() + while 1: + tm = int(sec_since_boot() * 1e9) + updated_messages.update(self.rcp.update(tm, True)) + if 0x445 in updated_messages: + break + else: + can_pub_radar = [] + + # TODO: can hang if no packets show up + while 1: + for a in messaging.drain_sock(self.logcan, wait_for_one=True): + canMonoTimes.append(a.logMonoTime) + can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) - # only run on the 0x445 packets, used for timing - if any(x[0] == 0x445 for x in can_pub_radar): - break + # only run on the 0x445 packets, used for timing + if any(x[0] == 0x445 for x in can_pub_radar): + break - updated_messages = self.rcp.update_can(can_pub_radar) + updated_messages = self.rcp.update_can(can_pub_radar) ret = car.RadarState.new_message() errors = [] @@ -56,6 +75,7 @@ class RadarInterface(object): for ii in updated_messages: cpt = self.rcp.vl[ii] + #print cpt if cpt['LONG_DIST'] < 255: if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() diff --git a/selfdrive/sensord/.gitignore b/selfdrive/sensord/.gitignore new file mode 100644 index 0000000000..e17675e254 --- /dev/null +++ b/selfdrive/sensord/.gitignore @@ -0,0 +1 @@ +sensord diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 596d0b0d12..7b3ffd256e 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:925d26ccc75ec4d7051b68fe98a929536c5224693f4e375fe20fb604299bc079 +oid sha256:58e476760c0f87f13b68b75d64860fb5c57b5509b4174d31f466074e0423e7eb size 918928 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index b0adabad8d..e6a1ce2144 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -41,6 +41,8 @@ navUpdate: [8028, true] qcomGnss: [8029, true] lidarPts: [8030, true] procLog: [8031, true] +gpsLocationExternal: [8032, true] +ubloxGnss: [8033, true] testModel: [8040, false] # manager -- base process to manage starting and stopping of all others diff --git a/selfdrive/test/plant/plant_ui.py b/selfdrive/test/plant/plant_ui.py deleted file mode 100755 index fed1234ca7..0000000000 --- a/selfdrive/test/plant/plant_ui.py +++ /dev/null @@ -1,122 +0,0 @@ -#!/usr/bin/env python -import pygame -from plant import Plant -from selfdrive.config import CruiseButtons -import numpy as np -import selfdrive.messaging as messaging -import math - -CAR_WIDTH = 2.0 -CAR_LENGTH = 4.5 - -METER = 8 - -def rot_center(image, angle): - """rotate an image while keeping its center and size""" - orig_rect = image.get_rect() - rot_image = pygame.transform.rotate(image, angle) - rot_rect = orig_rect.copy() - rot_rect.center = rot_image.get_rect().center - rot_image = rot_image.subsurface(rot_rect).copy() - return rot_image - -def car_w_color(c): - car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH)) - car.set_alpha(0) - car.fill((10,10,10)) - car.set_alpha(128) - pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1) - return car - -if __name__ == "__main__": - pygame.init() - display = pygame.display.set_mode((1000, 1000)) - pygame.display.set_caption('Plant UI') - - car = car_w_color((255,0,255)) - leadcar = car_w_color((255,0,0)) - - carx, cary, heading = 10.0, 50.0, 0.0 - - plant = Plant(100, distance_lead = 40.0) - - control_offset = 2.0 - control_pts = zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10) - - def pt_to_car(pt): - x,y = pt - x -= carx - y -= cary - rx = x * math.cos(-heading) + y * -math.sin(-heading) - ry = x * math.sin(-heading) + y * math.cos(-heading) - return rx, ry - - def pt_from_car(pt): - x,y = pt - rx = x * math.cos(heading) + y * -math.sin(heading) - ry = x * math.sin(heading) + y * math.cos(heading) - rx += carx - ry += cary - return rx, ry - - while 1: - if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25: - cruise_buttons = CruiseButtons.RES_ACCEL - else: - cruise_buttons = 0 - - md = messaging.new_message() - md.init('model') - md.model.frameId = 0 - for x in [md.model.path, md.model.leftLane, md.model.rightLane]: - x.points = [0.0]*50 - x.prob = 0.0 - x.std = 1.0 - - car_pts = map(pt_to_car, control_pts) - - print car_pts - - car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3) - md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist() - md.model.path.prob = 1.0 - Plant.model.send(md.to_bytes()) - - plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False) - - display.fill((10,10,10)) - - carx += plant.speed * plant.ts * math.cos(heading) - cary += plant.speed * plant.ts * math.sin(heading) - - # positive steering angle = steering right - print plant.angle_steer - heading += plant.angle_steer * plant.ts - print heading - - # draw my car - display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER)) - - # draw control pts - for x,y in control_pts: - pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2) - - # draw path - path_pts = zip(np.arange(0, 50), md.model.path.points) - - for x,y in path_pts: - x,y = pt_from_car((x,y)) - pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1) - - """ - # draw lead car - dl = (plant.distance_lead - plant.distance) + 4.5 - lx = carx + dl * math.cos(heading) - ly = cary + dl * math.sin(heading) - - display.blit(pygame.transform.rotate(leadcar, 90-math.degrees(heading)), (lx*METER, ly*METER)) - """ - - pygame.display.flip() - - diff --git a/selfdrive/test/plant/runtest.sh b/selfdrive/test/plant/runtest.sh index 49a638a5c6..a955d9f936 100755 --- a/selfdrive/test/plant/runtest.sh +++ b/selfdrive/test/plant/runtest.sh @@ -1,12 +1,14 @@ #!/bin/bash + +export OPTEST=1 +export OLD_CAN=1 + pushd ../../controls ./controlsd.py & pid1=$! ./radard.py & pid2=$! -./plannerd.py & -pid3=$! -trap "trap - SIGTERM && kill $pid1 && kill $pid2 && kill $pid3" SIGINT SIGTERM EXIT +trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT popd mkdir -p out MPLBACKEND=svg ./runtracks.py out diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 6c5303947a..732df0cfa9 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -6,7 +6,6 @@ from selfdrive.manager import manager_init, manager_prepare from selfdrive.manager import start_managed_process, kill_managed_process, get_running from selfdrive.manager import manage_baseui from selfdrive.config import CruiseButtons -from selfdrive.test.plant.plant import Plant from functools import wraps import time @@ -44,8 +43,10 @@ def with_processes(processes): return wrapper @phone_only -@with_processes(['controlsd', 'radard', 'plannerd']) +@with_processes(['controlsd', 'radard']) def test_controls(): + from selfdrive.test.plant.plant import Plant + # start the fake car for 2 seconds plant = Plant(100) for i in range(200): diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore new file mode 100644 index 0000000000..2c850c2090 --- /dev/null +++ b/selfdrive/ui/.gitignore @@ -0,0 +1 @@ +ui diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/spinner/spinner new file mode 100755 index 0000000000..b25ae81dbb --- /dev/null +++ b/selfdrive/ui/spinner/spinner @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:295a6226b3db60adfae573bcaf5760c0bed0d21b9942f5e6a06831a177b20c38 +size 205056 diff --git a/selfdrive/ui/spinner/spinner.c b/selfdrive/ui/spinner/spinner.c new file mode 100644 index 0000000000..f67e310a03 --- /dev/null +++ b/selfdrive/ui/spinner/spinner.c @@ -0,0 +1,77 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "nanovg.h" +#define NANOVG_GLES3_IMPLEMENTATION +#include "nanovg_gl.h" +#include "nanovg_gl_utils.h" + +#include "common/framebuffer.h" + + +int main(int argc, char** argv) { + int err; + + const char* spintext = NULL; + if (argc >= 2) { + spintext = argv[1]; + } + + // spinner + int fb_w, fb_h; + EGLDisplay display; + EGLSurface surface; + FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false, + &display, &surface, &fb_w, &fb_h); + assert(fb); + + NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES); + assert(vg); + + int font = nvgCreateFont(vg, "Bold", "../../assets/courbd.ttf"); + assert(font >= 0); + + for (int cnt = 0; ; cnt++) { + glClearColor(0.1, 0.1, 0.1, 1.0); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + nvgBeginFrame(vg, fb_w, fb_h, 1.0f); + + + for (int k=0; k<3; k++) { + float ang = (2*M_PI * (float)cnt / 120.0) + (k / 3.0) * 2*M_PI; + + nvgBeginPath(vg); + nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255)); + nvgStrokeWidth(vg, 5); + + nvgMoveTo(vg, fb_w/2 + 50 * cosf(ang), fb_h/2 + 50 * sinf(ang)); + nvgLineTo(vg, fb_w/2 + 15 * cosf(ang), fb_h/2 + 15 * sinf(ang)); + nvgMoveTo(vg, fb_w/2 - 15 * cosf(ang), fb_h/2 - 15 * sinf(ang)); + nvgLineTo(vg, fb_w/2 - 50 * cosf(ang), fb_h/2 - 50 * sinf(ang)); + nvgStroke(vg); + } + + if (spintext) { + nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); + nvgFontSize(vg, 96.0f); + nvgText(vg, fb_w / 2, fb_h*2/3, spintext, NULL); + } + + nvgEndFrame(vg); + + eglSwapBuffers(display, surface); + assert(glGetError() == GL_NO_ERROR); + } + + return 0; +} diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 075219b62a..0219f037da 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -61,6 +61,7 @@ typedef struct UIScene { uint8_t *bgr_front_ptr; int front_box_x, front_box_y, front_box_width, front_box_height; + uint64_t alert_ts; char alert_text1[1024]; char alert_text2[1024]; @@ -802,6 +803,11 @@ static void ui_draw_vision(UIState *s) { static void ui_draw_alerts(UIState *s) { const UIScene *scene = &s->scene; + // dont draw alerts that are outdated by > 20 secs + if ((nanos_since_boot() - scene->alert_ts) >= 20000000000ULL) { + return; + } + glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); @@ -1059,6 +1065,9 @@ static void ui_update(UIState *s) { s->scene.alert_text2[0] = '\0'; } s->scene.awareness_status = datad.awarenessStatus; + + s->scene.alert_ts = eventd.logMonoTime; + } else if (eventd.which == cereal_Event_live20) { struct cereal_Live20Data datad; cereal_read_Live20Data(&datad, eventd.live20); @@ -1220,6 +1229,8 @@ int main() { usleep(30000); } + set_awake(s, true); + err = pthread_join(connect_thread_handle, NULL); assert(err == 0); diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 96790aa3dd..905a555b67 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d4b8e99120b0e6f61b56e514e8b607ef797a6046c1cc2fffafe30c9231a65671 -size 16370760 +oid sha256:61b61563109853f59a5764cc520fef8d9f0d8d2a845ebbcf9381b6a64761f655 +size 16399784