diff --git a/Pipfile b/Pipfile index 6b9f7e95bf..7afffb1e94 100644 --- a/Pipfile +++ b/Pipfile @@ -99,6 +99,8 @@ simplejson = "*" python-logstash-async = "*" pandas = "*" seaborn = "*" +tensorflow-estimator = "==1.10.12" +pyproj = "*" [packages] overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"} diff --git a/Pipfile.lock b/Pipfile.lock index 30c43e2867..15416a7bf6 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "89070d7d9478ac9e6ad2c9848aaf724cb7362a1de4af9f8fb8c40be5f37c043d" + "sha256": "43f96719ff9a1b7eae6c7a88f903c4f5a137c2b195e3ba148f32767827b98710" }, "pipfile-spec": 6, "requires": { @@ -656,12 +656,12 @@ }, "aenum": { "hashes": [ - "sha256:058f0cfaf911899dc21b334362047df74ce989335dd8dff8e4be1a6313b15232", - "sha256:6af970173d9b4ac0384ad7d1cfe9523eeb9a3578793e1664090c13cb59df6469", - "sha256:80f14366578d84f6bccb0670259744cb3a7f2ab504480c306238a23cdd569457" + "sha256:0e3589654ef090784971f7778dcb74b08c9b4ef80b33267c00f82ddeedac179a", + "sha256:b12a7be3d89b270f266f8643aaa126404e5cdc0929bd6f09548b8eaed85e2aa1", + "sha256:e4dab068cbe00295bbc3660cd562221b008687d0f7a4c40fc2dd7a80002126a7" ], "index": "pypi", - "version": "==2.2.0" + "version": "==2.2.1" }, "amqp": { "hashes": [ @@ -817,18 +817,18 @@ }, "boto3": { "hashes": [ - "sha256:34a8ddb7247316be6ea94c7eeee41212312d250d99bf668fcd6748629b578622", - "sha256:71f3554cc69fa20be06cf20d6c9e0d8095d7c40695b48618676c3cd9a5ba0783" + "sha256:6a950bf98b22812896ea0f833a26d448acfdf43179f41f389d501af7a9fae328", + "sha256:cfbc062a76a7781af8e6a4ea26ebcafa3866872a8cceb05fdbf588c36e7848f0" ], "index": "pypi", - "version": "==1.9.189" + "version": "==1.9.195" }, "botocore": { "hashes": [ - "sha256:4febbf206d1dc8b8299aa211d8e382d5bf3f22097855b9f98d5e8c401ef8192b", - "sha256:b62ab3e4e98e075fc9e8e8fd4e8f5b92ebf311a6dc9f7578650938c7bc94e592" + "sha256:691627c2aeff0fcbd9237985717c28404a628181fd3e86b7be500bf2ee156007", + "sha256:c59e9981db9dfc54f0d22f731ca8de904760049a9c60d86dcedde84ae64ef4f0" ], - "version": "==1.12.189" + "version": "==1.12.195" }, "celery": { "hashes": [ @@ -1734,7 +1734,7 @@ "sha256:23953d55076df038541e648a53676fb24980f7a1be290cdda21300b3bc21dfb0", "sha256:552a91f381532e33cbd07c6a2655a21908088962bb8fa7239ecbcc6ad1140cc7" ], - "markers": "python_version == '2.7'", + "markers": "python_version == '2.6' or python_version == '2.7' or python_version == '3.0' or python_version == '3.1' or python_version == '3.2'", "version": "==1.5" }, "mpld3": { @@ -2264,6 +2264,32 @@ "index": "pypi", "version": "==0.1.17" }, + "pyproj": { + "hashes": [ + "sha256:1303b343a1c4d83d1ac17494571d958335c0a4597d4aa623cd64df1f36fb18bf", + "sha256:1fd2f756f43ba762f5a1ae769c6cc9ae6f919996bf80c621f331e4a5d89ae74f", + "sha256:2b3f59b16cde23c0d95b1d9ba3dd1c0be3937d877df3b47a522d3c9a74d31668", + "sha256:2e5d8b892782db22c3d6ffcb272c8ac0a18995e22d45db762a893ec64cc039d6", + "sha256:512a59bc20587f88df78a69872ed8cb4ec7905415ba85b4de0af35601770fba1", + "sha256:5aa89b04855e439e2b96caf1f49fa2ab60e6e7c407034284d31d808abed0618a", + "sha256:75b4c5eba36060a00867e16e81755a44e93a95b0bbaa4d6ee40f10c330b62108", + "sha256:860fde3429f364767c88a8e7f14f330e3fdace9f438e0ef1b112108a44c1eee8", + "sha256:86886f21af819553990f0da9ad7737d52cd32076dd277db34e40cf805919e839", + "sha256:9417d46f2b5e6ba79bd6adea0aae9659501c8a0350a5a98c0d9cb1442d45b0b4", + "sha256:9bb81141715832bb8682dda3c2f8f1ad0f43cf8aec5db52e0992c16c7664e227", + "sha256:afe150d8f0b351df7a357cfce50a8a5c0a44bdfeec5e08075f873cfeddf63004", + "sha256:cc2e21c09245c3eb47014c3d4c58888cc8d48a168a03533f9ed3f8b2ee82a352", + "sha256:cefb4a1a5d51cd04c9dd31bb18047c2dafcb86bc33a0347746b5a0ef82c071d8", + "sha256:cfa2cd0ee23bcfc9f165b4f8e9c5b7ba829c7b49175fb73420544619148d91af", + "sha256:d3a78ce50e6c50e701d65929dc04ab751edaeae660455a9fef3c2231d609f8c1", + "sha256:d772ee53c8d316de1b7f68244ab654f864553bca177357f8787a65dc09816ae3", + "sha256:ddd99d6f2c813e44721c22039ddf333e4b95e8d0794b03d9962c94c01c823934", + "sha256:ec89e68a0cf210af0cc2724b5f8601d4b6809ff0f556e16efc8c955e79672f7a", + "sha256:f118762ed0abe8e988a20020241fe1e6133edb960d2665a44d6f48acc6d8e7c7" + ], + "index": "pypi", + "version": "==2.2.1" + }, "pyrsistent": { "hashes": [ "sha256:50cffebc87ca91b9d4be2dcc2e479272bcb466b5a0487b6c271f7ddea6917e14" @@ -2424,10 +2450,10 @@ }, "qtconsole": { "hashes": [ - "sha256:4af84facdd6f00a6b9b2927255f717bb23ae4b7a20ba1d9ef0a5a5a8dbe01ae2", - "sha256:60d61d93f7d67ba2b265c6d599d413ffec21202fec999a952f658ff3a73d252b" + "sha256:6a85456af7a98b0f554d140922b7b6a219757b039adb2b95e847cf115eaa20ae", + "sha256:767eb9ec3f9943bc84270198b5ff95d2d86d68d6b57792fafa4df4fc6b16cd7c" ], - "version": "==4.5.1" + "version": "==4.5.2" }, "redis": { "hashes": [ @@ -2685,9 +2711,10 @@ }, "tensorflow-estimator": { "hashes": [ - "sha256:ca073f66063407a091d610ec1b22e39ea30248710198cc6f13769320bdbe3992" + "sha256:3e460f43682c7d789e5fe966630029558434d32502e632ee7f6703451258528c" ], - "version": "==1.14.0" + "index": "pypi", + "version": "==1.10.12" }, "tensorflow-gpu": { "hashes": [ diff --git a/RELEASES.md b/RELEASES.md index 07bbc6208d..f28977e922 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,15 @@ +Version 0.6.2 (2019-07-29) +======================== + * New driving model! + * Improve lane tracking with double lines + * Strongly improve stationary vehicle detection + * Strongly reduce cases of braking due to false leads + * Better lead tracking around turns + * Improve cut-in prediction by using neural network + * Improve lateral control on Toyota Camry and C-HR thanks to zorrobyte! + * Fix unintended openpilot disengagements on Jeep thanks to adhintz! + * Fix delayed transition to offroad when car is turned off + Version 0.6.1 (2019-07-21) ======================== * Remote SSH with comma prime and [ssh.comma.ai](https://ssh.comma.ai) diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 6161bfa528..c221675d77 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py deleted file mode 100644 index fbed218f7a..0000000000 --- a/common/kalman/ekf.py +++ /dev/null @@ -1,253 +0,0 @@ -# pylint: skip-file -from __future__ import print_function -import abc -import numpy as np -# 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. -# 2) __init__() to initialize self.state, self.covar, and self.process_noise appropriately - -# Alternatively, the existing implementations of EKF can be used (e.g. EKF2D) - -# Sensor classes are optionally used to pass measurement information into the EKF, to keep -# sensor parameters and processing methods for a each sensor together. -# Sensor classes have a read() method which takes raw sensor data and returns -# a SensorReading object, which can be passed to the EKF update() method. - -# For usage, see run_ekf1d.py in selfdrive/new for a simple example. -# ekf.predict(dt) should be called between update cycles with the time since it was last called. -# Ideally, predict(dt) should be called at a relatively constant rate. -# update() should be called once per sensor, and can be called multiple times between predict steps. -# Access and set the state of the filter directly with ekf.state and ekf.covar. - - -class SensorReading: - # Given a perfect model and no noise, data = obs_model * state - def __init__(self, data, covar, obs_model): - self.data = data - self.obs_model = obs_model - self.covar = covar - - def __repr__(self): - return "SensorReading(data={}, covar={}, obs_model={})".format( - repr(self.data), repr(self.covar), repr(self.obs_model)) - - -# A generic sensor class that does no pre-processing of data -class SimpleSensor: - # obs_model can be - # a full observation model matrix, or - # an integer or tuple of indices into ekf.state, indicating which variables are being directly observed - # covar can be - # a full covariance matrix - # a float or tuple of individual covars for each component of the sensor reading - # dims is the number of states in the EKF - def __init__(self, obs_model, covar, dims): - # Allow for integer covar/obs_model - if not hasattr(obs_model, "__len__"): - obs_model = (obs_model, ) - if not hasattr(covar, "__len__"): - covar = (covar, ) - - # Full observation model passed - if dims in np.array(obs_model).shape: - self.obs_model = np.asmatrix(obs_model) - self.covar = np.asmatrix(covar) - # Indices of unit observations passed - else: - self.obs_model = np.matlib.zeros((len(obs_model), dims)) - self.obs_model[:, list(obs_model)] = np.identity(len(obs_model)) - if np.asarray(covar).ndim == 2: - self.covar = np.asmatrix(covar) - elif len(covar) == len(obs_model): - self.covar = np.matlib.diag(covar) - else: - self.covar = np.matlib.identity(len(obs_model)) * covar - - def read(self, data, covar=None): - if covar: - self.covar = covar - return SensorReading(data, self.covar, self.obs_model) - - -class EKF: - __metaclass__ = abc.ABCMeta - - def __init__(self, debug=False): - self.DEBUG = debug - - - def __str__(self): - return "EKF(state={}, covar={})".format(self.state, self.covar) - - # Measurement update - # Reading should be a SensorReading object with data, covar, and obs_model attributes - def update(self, reading): - # Potential improvements: - # deal with negative covars - # add noise to really low covars to ensure stability - # use mahalanobis distance to reject outliers - # wrap angles after state updates and innovation - - # y = z - H*x - innovation = reading.data - reading.obs_model * self.state - - if self.DEBUG: - print("reading:\n",reading.data) - print("innovation:\n",innovation) - - # S = H*P*H' + R - innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar - - # K = P*H'*S^-1 - kalman_gain = self.covar * reading.obs_model.T * np.linalg.inv( - innovation_covar) - - if self.DEBUG: - print("gain:\n", kalman_gain) - print("innovation_covar:\n", innovation_covar) - print("innovation: ", innovation) - print("test: ", self.covar * reading.obs_model.T * ( - reading.obs_model * self.covar * reading.obs_model.T + reading.covar * - 0).I) - - # x = x + K*y - self.state += kalman_gain*innovation - - # print "covar", np.diag(self.covar) - #self.state[(roll_vel, yaw_vel, pitch_vel),:] = reading.data - - # Standard form: P = (I - K*H)*P - # self.covar = (self.identity - kalman_gain*reading.obs_model) * self.covar - - # Use the Joseph form for numerical stability: P = (I-K*H)*P*(I - K*H)' + K*R*K' - aux_mtrx = (self.identity - kalman_gain * reading.obs_model) - self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T - - if self.DEBUG: - print("After update") - print("state\n", self.state) - print("covar:\n",self.covar) - - def update_scalar(self, reading): - # like update but knowing that measurement is a scalar - # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot - - # innovation = reading.data - np.matmul(reading.obs_model, self.state) - # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar - # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar - # self.state += np.matmul(kalman_gain, innovation) - # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model) - # self.covar = np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T)) - - # written without np.matmul - es = np.einsum - ABC_T = "ij,jk,lk->il" - AB_T = "ij,kj->ik" - AB = "ij,jk->ik" - innovation = reading.data - es(AB, reading.obs_model, self.state) - innovation_covar = es(ABC_T, reading.obs_model, self.covar, - reading.obs_model) + reading.covar - kalman_gain = es(AB_T, self.covar, reading.obs_model) / innovation_covar - - self.state += es(AB, kalman_gain, innovation) - aux_mtrx = self.identity - es(AB, kalman_gain, reading.obs_model) - self.covar = es(ABC_T, aux_mtrx, self.covar, aux_mtrx) + \ - es(ABC_T, kalman_gain, reading.covar, kalman_gain) - - # Prediction update - def predict(self, dt): - es = np.einsum - ABC_T = "ij,jk,lk->il" - AB = "ij,jk->ik" - - # State update - transfer_fun, transfer_fun_jacobian = self.calc_transfer_fun(dt) - - # self.state = np.matmul(transfer_fun, self.state) - # self.covar = np.matmul(np.matmul(transfer_fun_jacobian, self.covar), transfer_fun_jacobian.T) + self.process_noise * dt - - # x = f(x, u), written in the form x = A(x, u)*x - self.state = es(AB, transfer_fun, self.state) - - # P = J*P*J' + Q - self.covar = es(ABC_T, transfer_fun_jacobian, self.covar, - transfer_fun_jacobian) + self.process_noise * dt #!dt - - #! 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 - The transfer function and jacobian should both be a numpy matrix of size DIMSxDIMS - - The transfer function matrix A should satisfy the state-update equation - x_(k+1) = A * x_k - - The jacobian J is the direct jacobian A*x_k. For linear systems J=A. - - Current implementations calculate A and J as functions of state. Control input - can be added trivially by adding a control parameter to predict() and calc_tranfer_update(), - and using it during calculation of A and J - """ - - -class FastEKF1D(EKF): - """Fast version of EKF for 1D problems with scalar readings.""" - - def __init__(self, dt, var_init, Q): - super(FastEKF1D, self).__init__(False) - self.state = [0, 0] - self.covar = [var_init, var_init, 0] - - # Process Noise - self.dtQ0 = dt * Q[0] - self.dtQ1 = dt * Q[1] - - def update(self, reading): - raise NotImplementedError - - def update_scalar(self, reading): - # TODO(mgraczyk): Delete this for speed. - # assert np.all(reading.obs_model == [1, 0]) - - rcov = reading.covar[0, 0] - - x = self.state - S = self.covar - - innovation = reading.data - x[0] - innovation_covar = S[0] + rcov - - k0 = S[0] / innovation_covar - k1 = S[2] / innovation_covar - - x[0] += k0 * innovation - x[1] += k1 * innovation - - mk = 1 - k0 - S[1] += k1 * (k1 * (S[0] + rcov) - 2 * S[2]) - S[2] = mk * (S[2] - k1 * S[0]) + rcov * k0 * k1 - S[0] = mk * mk * S[0] + rcov * k0 * k0 - - def predict(self, dt): - # State update - x = self.state - - x[0] += dt * x[1] - - # P = J*P*J' + Q - S = self.covar - S[0] += dt * (2 * S[2] + dt * S[1]) + self.dtQ0 - S[2] += dt * S[1] - S[1] += self.dtQ1 - - # Clip covariance to avoid explosions - S = max(-1e10, min(S, 1e10)) - - def calc_transfer_fun(self, dt): - tf = np.identity(2) - tf[0, 1] = dt - tfj = tf - return tf, tfj diff --git a/common/kalman/tests/test_ekf.py b/common/kalman/tests/test_ekf.py deleted file mode 100644 index 3d6da323d4..0000000000 --- a/common/kalman/tests/test_ekf.py +++ /dev/null @@ -1,116 +0,0 @@ -import numpy as np -import numpy.matlib -import unittest -import timeit - -from common.kalman.ekf import EKF, SimpleSensor, FastEKF1D - -class TestEKF(EKF): - def __init__(self, var_init, Q): - super(TestEKF, self).__init__(False) - self.identity = numpy.matlib.identity(2) - self.state = numpy.matlib.zeros((2, 1)) - self.covar = self.identity * var_init - - self.process_noise = numpy.matlib.diag(Q) - - def calc_transfer_fun(self, dt): - tf = numpy.matlib.identity(2) - tf[0, 1] = dt - return tf, tf - - -class EKFTest(unittest.TestCase): - def test_update_scalar(self): - ekf = TestEKF(1e3, [0.1, 1]) - dt = 1. / 100 - - sensor = SimpleSensor(0, 1, 2) - readings = map(sensor.read, np.arange(100, 300)) - - for reading in readings: - ekf.update_scalar(reading) - ekf.predict(dt) - - np.testing.assert_allclose(ekf.state, [[300], [100]], 1e-4) - np.testing.assert_allclose( - ekf.covar, - np.asarray([[0.0563, 0.10278], [0.10278, 0.55779]]), - atol=1e-4) - - def test_unbiased(self): - ekf = TestEKF(1e3, [0., 0.]) - dt = np.float64(1. / 100) - - sensor = SimpleSensor(0, 1, 2) - readings = map(sensor.read, np.arange(1000)) - - for reading in readings: - ekf.update_scalar(reading) - ekf.predict(dt) - - np.testing.assert_allclose(ekf.state, [[1000.], [100.]], 1e-4) - - -class FastEKF1DTest(unittest.TestCase): - def test_correctness(self): - dt = 1. / 100 - reading = SimpleSensor(0, 1, 2).read(100) - - ekf = TestEKF(1e3, [0.1, 1]) - fast_ekf = FastEKF1D(dt, 1e3, [0.1, 1]) - - ekf.update_scalar(reading) - fast_ekf.update_scalar(reading) - self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0]) - self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1]) - self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0]) - self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2]) - self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1]) - - ekf.predict(dt) - fast_ekf.predict(dt) - self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0]) - self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1]) - self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0]) - self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2]) - self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1]) - - def test_speed(self): - setup = """ -import numpy as np -from common.kalman.tests.test_ekf import TestEKF -from common.kalman.ekf import SimpleSensor, FastEKF1D - -dt = 1. / 100 -reading = SimpleSensor(0, 1, 2).read(100) - -var_init, Q = 1e3, [0.1, 1] -ekf = TestEKF(var_init, Q) -fast_ekf = FastEKF1D(dt, var_init, Q) - """ - - timeit.timeit(""" -ekf.update_scalar(reading) -ekf.predict(dt) - """, setup=setup, number=1000) - - ekf_speed = timeit.timeit(""" -ekf.update_scalar(reading) -ekf.predict(dt) - """, setup=setup, number=20000) - - timeit.timeit(""" -fast_ekf.update_scalar(reading) -fast_ekf.predict(dt) - """, setup=setup, number=1000) - - fast_ekf_speed = timeit.timeit(""" -fast_ekf.update_scalar(reading) -fast_ekf.predict(dt) - """, setup=setup, number=20000) - - assert fast_ekf_speed < ekf_speed / 4 - -if __name__ == "__main__": - unittest.main() diff --git a/common/params.py b/common/params.py index 963debd091..7f1b3ab98b 100755 --- a/common/params.py +++ b/common/params.py @@ -53,6 +53,7 @@ keys = { "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CompletedTrainingVersion": [TxType.PERSISTENT], "ControlsParams": [TxType.PERSISTENT], "DoUninstall": [TxType.CLEAR_ON_MANAGER_START], @@ -70,6 +71,7 @@ keys = { "IsUploadRawEnabled": [TxType.PERSISTENT], "IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT], "LimitSetSpeed": [TxType.PERSISTENT], + "LimitSetSpeedNeural": [TxType.PERSISTENT], "LiveParameters": [TxType.PERSISTENT], "LongitudinalControl": [TxType.PERSISTENT], "Passive": [TxType.PERSISTENT], diff --git a/common/realtime.py b/common/realtime.py index 8bcd06966f..6fe64d0d72 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -20,6 +20,7 @@ DT_CTRL = 0.01 # controlsd DT_PLAN = 0.05 # mpc DT_MDL = 0.05 # model DT_DMON = 0.1 # driver monitoring +DT_TRML = 0.5 # thermald and manager ffi = FFI() diff --git a/common/vin.py b/common/vin.py index 29357e34bd..989aa0fa13 100755 --- a/common/vin.py +++ b/common/vin.py @@ -1,61 +1,8 @@ #!/usr/bin/env python -import time -from common.realtime import sec_since_boot import selfdrive.messaging as messaging from selfdrive.boardd.boardd import can_list_to_can_capnp -def get_vin(logcan, sendcan): - - # works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru; - # Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII - query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0], - [0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]] - - cnts = [1, 2] # Number of messages to wait for at each iteration - vin_valid = True - - dat = [] - for i in range(len(query_msg)): - cnt = 0 - sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan')) - got_response = False - t_start = sec_since_boot() - while sec_since_boot() - t_start < 0.05 and not got_response: - for a in messaging.drain_sock(logcan): - for can in a.can: - if can.src == 0 and can.address == 0x7e8: - vin_valid = vin_valid and is_vin_response_valid(can.dat, i, cnt) - dat += can.dat[2:] if i == 0 else can.dat[1:] - cnt += 1 - if cnt == cnts[i]: - got_response = True - time.sleep(0.01) - - return "".join(dat[3:]) if vin_valid else "" - -""" -if 'vin' not in gctx: - print "getting vin" - gctx['vin'] = query_vin()[3:] - print "got VIN %s" % (gctx['vin'],) - cloudlog.info("got VIN %s" % (gctx['vin'],)) - -# *** determine platform based on VIN **** -if vin.startswith("19UDE2F36G"): - print "ACURA ILX 2016" - self.civic = False -else: - # TODO: add Honda check explicitly - print "HONDA CIVIC 2016" - self.civic = True - -# *** special case VIN of Acura test platform -if vin == "19UDE2F36GA001322": - print "comma.ai test platform detected" - # it has a gas interceptor and a torque mod - self.torque_mod = True -""" - +VIN_UNKNOWN = "0" * 17 # sanity checks on response messages from vin query def is_vin_response_valid(can_dat, step, cnt): @@ -84,12 +31,71 @@ def is_vin_response_valid(can_dat, step, cnt): return True +class VinQuery(): + def __init__(self, bus): + self.bus = bus + # works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru; + # Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII + self.query_ext_msgs = [[0x18DB33F1, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus], + [0x18DA10f1, 0, '\x30'.ljust(8, "\x00"), bus]] + self.query_nor_msgs = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus], + [0x7e0, 0, '\x30'.ljust(8, "\x00"), bus]] + + self.cnts = [1, 2] # number of messages to wait for at each iteration + self.step = 0 + self.cnt = 0 + self.responded = False + self.never_responded = True + self.dat = [] + self.vin = VIN_UNKNOWN + + def check_response(self, msg): + # have we got a VIN query response? + if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]: + self.never_responded = False + # basic sanity checks on ISO-TP response + if is_vin_response_valid(msg.dat, self.step, self.cnt): + self.dat += msg.dat[2:] if self.step == 0 else msg.dat[1:] + self.cnt += 1 + if self.cnt == self.cnts[self.step]: + self.responded = True + self.step += 1 + + def send_query(self, sendcan): + # keep sending VIN qury if ECU isn't responsing. + # sendcan is probably not ready due to the zmq slow joiner syndrome + if self.never_responded or (self.responded and self.step < len(self.cnts)): + sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan')) + sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan')) + self.responded = False + self.cnt = 0 + + def get_vin(self): + # only report vin if procedure is finished + if self.step == len(self.cnts) and self.cnt == self.cnts[-1]: + self.vin = "".join(self.dat[3:]) + return self.vin + + +def get_vin(logcan, sendcan, bus, query_time=1.): + vin_query = VinQuery(bus) + frame = 0 + + # 1s max of VIN query time + while frame < query_time * 100: + a = messaging.recv_one(logcan) + + for can in a.can: + vin_query.check_response(can) + + vin_query.send_query(sendcan) + frame += 1 + + return vin_query.get_vin() + + if __name__ == "__main__": - import zmq from selfdrive.services import service_list - context = zmq.Context() - logcan = messaging.sub_sock(context, service_list['can'].port) - sendcan = messaging.pub_sock(context, service_list['sendcan'].port) - time.sleep(1.) # give time to sendcan socket to start - - print get_vin(logcan, sendcan) + logcan = messaging.sub_sock(service_list['can'].port) + sendcan = messaging.pub_sock(service_list['sendcan'].port) + print get_vin(logcan, sendcan, 0) diff --git a/models/driving_model.dlc b/models/driving_model.dlc index 3cceccf116..d06aad6190 100644 Binary files a/models/driving_model.dlc and b/models/driving_model.dlc differ diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc index b860a1185a..47e71afe2e 100644 Binary files a/models/monitoring_model.dlc and b/models/monitoring_model.dlc differ diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 779ca7b279..f4ca033f5b 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -59,7 +59,8 @@ pthread_mutex_t usb_lock; bool spoofing_started = false; bool fake_send = false; bool loopback_can = false; -bool is_grey_panda = false; +cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; +bool is_pigeon = false; pthread_t safety_setter_thread_handle = -1; pthread_t pigeon_thread_handle = -1; @@ -69,6 +70,29 @@ void pigeon_init(); void *pigeon_thread(void *crap); void *safety_setter_thread(void *s) { + char *value_vin; + size_t value_vin_sz = 0; + + // switch to no_output when CarVin param is read + while (1) { + if (do_exit) return NULL; + const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz); + if (value_vin_sz > 0) { + // sanity check VIN format + assert(value_vin_sz == 17); + break; + } + usleep(100*1000); + } + LOGW("got CarVin %s", value_vin); + + pthread_mutex_lock(&usb_lock); + + // VIN qury done, stop listening to OBDII + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); + + pthread_mutex_unlock(&usb_lock); + char *value; size_t value_sz = 0; @@ -151,7 +175,7 @@ void *safety_setter_thread(void *s) { // must be called before threads or with mutex bool usb_connect() { int err; - unsigned char is_pigeon[1] = {0}; + unsigned char hw_query[1] = {0}; dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc); if (dev_handle == NULL) { goto fail; } @@ -184,11 +208,12 @@ bool usb_connect() { assert(err == 0); } - libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, is_pigeon, 1, TIMEOUT); + libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT); - if (is_pigeon[0]) { - LOGW("grey panda detected"); - is_grey_panda = true; + hw_type = (cereal::HealthData::HwType)(hw_query[0]); + is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA); + if (is_pigeon) { + LOGW("panda with gps detected"); pigeon_needs_init = true; if (pigeon_thread_handle == -1) { err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL); @@ -280,12 +305,13 @@ void can_health(void *s) { struct __attribute__((packed)) health { uint32_t voltage; uint32_t current; - uint8_t started; - uint8_t controls_allowed; - uint8_t gas_interceptor_detected; uint32_t can_send_errs; uint32_t can_fwd_errs; uint32_t gmlan_send_errs; + uint8_t started; + uint8_t controls_allowed; + uint8_t gas_interceptor_detected; + uint8_t car_harness_status_pkt; } health; // recv from board @@ -293,7 +319,9 @@ void can_health(void *s) { do { cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT); - if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); } + if (cnt != sizeof(health)) { + handle_usb_issue(cnt, __func__); + } } while(cnt != sizeof(health)); pthread_mutex_unlock(&usb_lock); @@ -314,15 +342,23 @@ void can_health(void *s) { } healthData.setControlsAllowed(health.controls_allowed); healthData.setGasInterceptorDetected(health.gas_interceptor_detected); - healthData.setIsGreyPanda(is_grey_panda); + healthData.setHasGps(is_pigeon); healthData.setCanSendErrs(health.can_send_errs); healthData.setCanFwdErrs(health.can_fwd_errs); healthData.setGmlanSendErrs(health.gmlan_send_errs); + healthData.setHwType(hw_type); // send to health auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); zmq_send(s, bytes.begin(), bytes.size(), 0); + + pthread_mutex_lock(&usb_lock); + + // send heartbeat back to panda + libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT); + + pthread_mutex_unlock(&usb_lock); } @@ -447,10 +483,10 @@ void *can_health_thread(void *crap) { void *publisher = zmq_socket(context, ZMQ_PUB); zmq_bind(publisher, "tcp://*:8011"); - // run at 1hz + // run at 2hz while (!do_exit) { can_health(publisher); - usleep(1000*1000); + usleep(500*1000); } return NULL; } @@ -502,7 +538,7 @@ void pigeon_set_baud(int baud) { void pigeon_init() { usleep(1000*1000); - LOGW("grey panda start"); + LOGW("panda GPS start"); // power off pigeon pigeon_set_power(0); @@ -543,7 +579,7 @@ void pigeon_init() { pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"); pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"); - LOGW("grey panda is ready to fly"); + LOGW("panda GPS on"); } static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) { diff --git a/selfdrive/can/tests/test_packer_honda.py b/selfdrive/can/tests/test_packer_honda.py index 87d62244e2..b1744155be 100644 --- a/selfdrive/can/tests/test_packer_honda.py +++ b/selfdrive/can/tests/test_packer_honda.py @@ -16,6 +16,9 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all commands, randomize the params. for _ in xrange(1000): + is_panda_black = False + car_fingerprint = HONDA_BOSCH[0] + apply_brake = (random.randint(0, 2) % 2 == 0) pump_on = (random.randint(0, 2) % 2 == 0) pcm_override = (random.randint(0, 2) % 2 == 0) @@ -23,33 +26,31 @@ class TestPackerMethods(unittest.TestCase): chime = random.randint(0, 65536) fcw = random.randint(0, 65536) idx = random.randint(0, 65536) - m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx) - m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx) + m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black) + m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black) self.assertEqual(m_old, m) apply_steer = (random.randint(0, 2) % 2 == 0) lkas_active = (random.randint(0, 2) % 2 == 0) - car_fingerprint = HONDA_BOSCH[0] idx = random.randint(0, 65536) - m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx) - m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx) + m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black) + m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black) self.assertEqual(m_old, m) pcm_speed = random.randint(0, 65536) hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536), 0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536)) - car_fingerprint = HONDA_BOSCH[0] idx = random.randint(0, 65536) is_metric = (random.randint(0, 2) % 2 == 0) - m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx) - m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx) + m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black) + m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black) self.assertEqual(m_old, m) button_val = random.randint(0, 65536) idx = random.randint(0, 65536) - m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx) - m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx) + m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black) + m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black) self.assertEqual(m_old, m) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 155e443e1c..45b9f94e12 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -1,8 +1,9 @@ import os -from common.vin import is_vin_response_valid +from cereal import car +from common.params import Params +from common.vin import get_vin, VIN_UNKNOWN from common.basedir import BASEDIR from common.fingerprints import eliminate_incompatible_cars, all_known_cars -from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging @@ -51,107 +52,85 @@ def _get_interface_names(): interfaces = load_interfaces(_get_interface_names()) def only_toyota_left(candidate_cars): - return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) + return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0 # BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai # **** for use live only **** -def fingerprint(logcan, sendcan): +def fingerprint(logcan, sendcan, is_panda_black): if os.getenv("SIMULATOR2") is not None: return ("simulator2", None, "") elif os.getenv("SIMULATOR") is not None: return ("simulator", None, "") - finger = {0: {}, 2:{}} # collect on bus 0 or 2 - cloudlog.warning("waiting for fingerprint...") - candidate_cars = all_known_cars() - can_seen_frame = None - can_seen = False - - # works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru; - # Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII - vin_query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0], - [0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]] - - vin_cnts = [1, 2] # number of messages to wait for at each iteration - vin_step = 0 - vin_cnt = 0 - vin_responded = False - vin_never_responded = True - vin_dat = [] - vin = "" + params = Params() + car_params = params.get("CarParams") + if car_params is not None: + # use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode + car_params = car.CarParams.from_bytes(car_params) + vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin + elif is_panda_black: + # Vin query only reliably works thorugh OBDII + vin = get_vin(logcan, sendcan, 1) + else: + vin = VIN_UNKNOWN + + cloudlog.warning("VIN %s", vin) + Params().put("CarVin", vin) + + finger = {i: {} for i in range(0, 4)} # collect on all buses + candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 + frame_fingerprint = 10 # 0.1s + car_fingerprint = None + done = False - while True: + while not done: a = messaging.recv_one(logcan) for can in a.can: - can_seen = True - - # have we got a VIN query response? - if can.src == 0 and can.address == 0x7e8: - vin_never_responded = False - # basic sanity checks on ISO-TP response - if is_vin_response_valid(can.dat, vin_step, vin_cnt): - vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:] - vin_cnt += 1 - if vin_cnt == vin_cnts[vin_step]: - vin_responded = True - vin_step += 1 - - # ignore everything not on bus 0 and with more than 11 bits, - # which are ussually sporadic and hard to include in fingerprints. - # also exclude VIN query response on 0x7e8. + # need to independently try to fingerprint both bus 0 and 1 to work + # for the combo black_panda and honda_bosch. Ignore extended messages + # and VIN query response. # Include bus 2 for toyotas to disambiguate cars using camera messages # (ideally should be done for all cars but we can't for Honda Bosch) - if (can.src == 0 or (only_toyota_left(candidate_cars) and can.src == 2)) and \ - can.address < 0x800 and can.address != 0x7e8: - finger[can.src][can.address] = len(can.dat) - candidate_cars = eliminate_incompatible_cars(can, candidate_cars) - - if can_seen_frame is None and can_seen: - can_seen_frame = frame - - # if we only have one car choice and the time_fingerprint since we got our first - # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not - # broadcast immediately - if len(candidate_cars) == 1 and can_seen_frame is not None: - time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1 - if (frame - can_seen_frame) > (time_fingerprint * 100): - break - - # bail if no cars left or we've been waiting for more than 2s since can_seen - elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200): - return None, finger, "" - - # keep sending VIN qury if ECU isn't responsing. - # sendcan is probably not ready due to the zmq slow joiner syndrome - # TODO: VIN query temporarily disabled until we have the harness - if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))): - sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan')) - vin_responded = False - vin_cnt = 0 + for b in candidate_cars: + if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \ + can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]: + finger[can.src][can.address] = len(can.dat) + candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) + + # if we only have one car choice and the time since we got our first + # message has elapsed, exit + for b in candidate_cars: + # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately + if only_toyota_left(candidate_cars[b]): + frame_fingerprint = 100 # 1s + if len(candidate_cars[b]) == 1: + if frame > frame_fingerprint: + # fingerprint done + car_fingerprint = candidate_cars[b][0] + + # bail if no cars left or we've been waiting for more than 2s + failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200 + succeeded = car_fingerprint is not None + done = failed or succeeded frame += 1 - # only report vin if procedure is finished - if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]: - vin = "".join(vin_dat[3:]) - - cloudlog.warning("fingerprinted %s", candidate_cars[0]) - cloudlog.warning("VIN %s", vin) - return candidate_cars[0], finger, vin + cloudlog.warning("fingerprinted %s", car_fingerprint) + return car_fingerprint, finger, vin -def get_car(logcan, sendcan): +def get_car(logcan, sendcan, is_panda_black=False): - candidate, fingerprints, vin = fingerprint(logcan, sendcan) + candidate, fingerprints, vin = fingerprint(logcan, sendcan, is_panda_black) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" CarInterface, CarController = interfaces[candidate] - params = CarInterface.get_params(candidate, fingerprints[0], vin) + car_params = CarInterface.get_params(candidate, fingerprints[0], vin, is_panda_black) - return CarInterface(params, CarController), params + return CarInterface(car_params, CarController), car_params diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 24acc76a4b..6368847c8f 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -27,7 +27,7 @@ def get_can_parser(CP): ("DOOR_OPEN_RL", "DOORS", 0), ("DOOR_OPEN_RR", "DOORS", 0), ("BRAKE_PRESSED_2", "BRAKE_2", 0), - ("ACCEL_PEDAL", "ACCEL_PEDAL_MSG", 0), + ("ACCEL_134", "ACCEL_GAS_134", 0), ("SPEED_LEFT", "SPEED_1", 0), ("SPEED_RIGHT", "SPEED_1", 0), ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), @@ -112,7 +112,7 @@ class CarState(object): self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0) self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only - self.pedal_gas = cp.vl["ACCEL_PEDAL_MSG"]['ACCEL_PEDAL'] + self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134'] self.car_gas = self.pedal_gas self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 65a60904f5..6cc4061d6c 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -38,13 +38,14 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "chrysler" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black ret.safetyModel = car.CarParams.SafetyModel.chrysler @@ -94,7 +95,7 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] - ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) + ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black print("ECU Camera Simulated: {0}".format(ret.enableCamera)) ret.openpilotLongitudinalControl = False diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 188931a060..f132881542 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -44,7 +44,7 @@ FINGERPRINTS = { ], CAR.JEEP_CHEROKEE: [ # JEEP GRAND CHEROKEE V6 2018 - {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, + {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, # Jeep Grand Cherokee 2017 Trailhawk {257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, ], diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index ca0f9f830f..59acc84324 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -38,13 +38,14 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "ford" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black ret.safetyModel = car.CarParams.SafetyModel.ford @@ -87,7 +88,7 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] - ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) + ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) or is_panda_black ret.openpilotLongitudinalControl = False cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 71b0efad29..449b8ae5dc 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -46,19 +46,20 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "gm" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black ret.enableCruise = False # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). - ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) + ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black ret.openpilotLongitudinalControl = ret.enableCamera tire_stiffness_factor = 0.444 # not optimized yet diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 3a5703e117..de8ae50787 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -149,19 +149,19 @@ class CarController(object): # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, - lkas_active, CS.CP.carFingerprint, idx)) + lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame//10) % 4 - can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx)) + can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) elif CS.stopped: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: # Send gas and brake commands. @@ -170,7 +170,7 @@ class CarController(object): ts = frame * DT_CTRL pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, - pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) + pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index e2bb82c73c..07ceb46c3e 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -154,7 +154,8 @@ def get_can_signals(CP): def get_can_parser(CP): signals, checks = get_can_signals(CP) - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0 + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt) def get_cam_can_parser(CP): @@ -165,9 +166,8 @@ def get_cam_can_parser(CP): if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: checks = [(0x194, 100)] - cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2 - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus) + bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) class CarState(object): def __init__(self, CP): diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 3955bfdcdc..a4c0e98542 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -19,7 +19,15 @@ def fix(msg, addr): return msg2 -def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx): +def get_pt_bus(car_fingerprint, is_panda_black): + return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0 + + +def get_lkas_cmd_bus(car_fingerprint, is_panda_black): + return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0 + + +def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 brake_rq = apply_brake > 0 @@ -38,27 +46,25 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work "FCW": fcw << 1, } - return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx) + bus = get_pt_bus(car_fingerprint, is_panda_black) + return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx) -def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx): +def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black): values = { "STEER_TORQUE": apply_steer if lkas_active else 0, "STEER_TORQUE_REQUEST": lkas_active, } - # Set bus 2 for accord and new crv. - bus = 2 if car_fingerprint in HONDA_BOSCH else 0 + bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black) return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) -def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx): +def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black): commands = [] - bus = 0 + bus_pt = get_pt_bus(car_fingerprint, is_panda_black) + bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black) - # Bosch sends commands to bus 2. - if car_fingerprint in HONDA_BOSCH: - bus = 2 - else: + if car_fingerprint not in HONDA_BOSCH: acc_hud_values = { 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, 'PCM_GAS': hud.pcm_accel, @@ -70,7 +76,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx): 'SET_ME_X01_2': 1, 'SET_ME_X01': 1, } - commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) + commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx)) lkas_hud_values = { 'SET_ME_X41': 0x41, @@ -79,23 +85,23 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx): 'SOLID_LANES': hud.lanes, 'BEEP': hud.beep, } - commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values, idx)) + commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx)) if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY): - radar_hud_values = { 'ACC_ALERTS': hud.acc_alert, 'LEAD_SPEED': 0x1fe, # What are these magic values 'LEAD_STATE': 0x7, 'LEAD_DISTANCE': 0x1e, } - commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx)) + commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx)) return commands -def spam_buttons_command(packer, button_val, idx): +def spam_buttons_command(packer, button_val, idx, car_fingerprint, is_panda_black): values = { 'CRUISE_BUTTONS': button_val, 'CRUISE_SETTING': 0, } - return packer.make_can_msg("SCM_BUTTONS", 0, values, idx) + bus = get_pt_bus(car_fingerprint, is_panda_black) + return packer.make_can_msg("SCM_BUTTONS", bus, values, idx) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 7ad72cdc48..9e7b64885e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -129,12 +129,13 @@ class CarInterface(object): return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "honda" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModel.hondaBosch @@ -143,7 +144,7 @@ class CarInterface(object): ret.openpilotLongitudinalControl = False else: ret.safetyModel = car.CarParams.SafetyModel.honda - ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) + ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black ret.enableGasInterceptor = 0x201 in fingerprint ret.openpilotLongitudinalControl = ret.enableCamera @@ -167,7 +168,7 @@ class CarInterface(object): ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT - ret.steerRatio = 14.63 # 10.93 is end-to-end spec + ret.steerRatio = 15.38 # 10.93 is end-to-end spec tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] @@ -187,7 +188,7 @@ class CarInterface(object): ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 15.96 # 11.82 is spec end-to-end + ret.steerRatio = 16.33 # 11.82 is spec end-to-end tire_stiffness_factor = 0.8467 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -213,7 +214,7 @@ class CarInterface(object): ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.3 # as spec + ret.steerRatio = 16.89 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -293,7 +294,7 @@ class CarInterface(object): ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight ret.wheelbase = 2.82 ret.centerToFront = ret.wheelbase * 0.428 # average weight distribution - ret.steerRatio = 16.0 # as spec + ret.steerRatio = 17.25 # as spec tire_stiffness_factor = 0.444 # not optimized yet ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 8b4d13bf75..c33300c43f 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -73,6 +73,9 @@ class CAR: PILOT_2019 = "HONDA PILOT 2019 ELITE" RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION" +# diag message that in some Nidec cars only appear with 1s freq if VIN query is performed +DIAG_MSGS = {1600: 5, 1601: 8} + FINGERPRINTS = { CAR.ACCORD: [{ 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8 @@ -139,6 +142,12 @@ FINGERPRINTS = { }] } +# add DIAG_MSGS to fingerprints +for c in FINGERPRINTS: + for f, _ in enumerate(FINGERPRINTS[c]): + for d in DIAG_MSGS: + FINGERPRINTS[c][f][d] = DIAG_MSGS[d] + DBC = { CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None), CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None), diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 4e1b2e6be2..c367324dac 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -40,13 +40,14 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "hyundai" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.hyundai ret.enableCruise = True # stock acc @@ -141,7 +142,7 @@ class CarInterface(object): ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) + ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black ret.openpilotLongitudinalControl = False ret.steerLimitAlert = False diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 3f47b9b00a..b703776c0b 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -4,7 +4,6 @@ from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging -from common.realtime import Ratekeeper # mocked car interface to work with chffrplus TS = 0.01 # 100Hz @@ -30,8 +29,6 @@ class CarInterface(object): self.yaw_rate = 0. self.yaw_rate_meas = 0. - self.rk = Ratekeeper(100, print_delay_threshold=2. / 1000) - @staticmethod def compute_gb(accel, speed): return accel @@ -41,7 +38,7 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() @@ -81,8 +78,6 @@ class CarInterface(object): # returns a car.CarState def update(self, c, can_strings): - self.rk.keep_time() - # get basic data from phone and gps since CAN isn't connected sensors = messaging.recv_sock(self.sensor) if sensors is not None: diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index ad8d1d5a48..c5fe0062a7 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -38,12 +38,13 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "subaru" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black ret.safetyModel = car.CarParams.SafetyModel.subaru ret.enableCruise = True diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 40be422db1..920a8ab5d3 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -3,7 +3,7 @@ from common.kalman.simple_kalman import KF1D from selfdrive.can.can_define import CANDefine from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR +from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR def parse_gear_shifter(gear, vals): @@ -19,6 +19,7 @@ def get_can_parser(CP): signals = [ # sig_name, sig_address, default + ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), ("GEAR", "GEAR_PACKET", 0), ("BRAKE_PRESSED", "BRAKE_MODULE", 0), ("GAS_PEDAL", "GAS_PEDAL", 0), @@ -59,10 +60,8 @@ def get_can_parser(CP): ("EPS_STATUS", 25), ] - if CP.carFingerprint in TSS2_CAR: + if CP.carFingerprint in NO_DSU_CAR: signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)] - else: - signals += [("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0)] if CP.carFingerprint == CAR.PRIUS: signals += [("STATE", "AUTOPARK_STATUS", 0)] @@ -94,6 +93,8 @@ class CarState(object): self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR'] self.left_blinker_on = 0 self.right_blinker_on = 0 + self.angle_offset = 0. + self.init_angle_offset = False # initialize can parser self.car_fingerprint = CP.carFingerprint @@ -144,6 +145,14 @@ class CarState(object): if self.CP.carFingerprint in TSS2_CAR: self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] + elif self.CP.carFingerprint in NO_DSU_CAR: + # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start. + # need to apply an offset as soon as the steering angle measurements are both received + self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset + angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset: + self.init_angle_offset = True + self.angle_offset = self.angle_steers - angle_wheel else: self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 5e5cb2c839..dc3fe54499 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -40,13 +40,14 @@ class CarInterface(object): return 1.0 @staticmethod - def get_params(candidate, fingerprint, vin=""): + def get_params(candidate, fingerprint, vin="", is_panda_black=False): ret = car.CarParams.new_message() ret.carName = "toyota" ret.carFingerprint = candidate ret.carVin = vin + ret.isPandaBlack = is_panda_black ret.safetyModel = car.CarParams.SafetyModel.toyota @@ -62,7 +63,7 @@ class CarInterface(object): stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 - ret.steerRatio = 15.00 # unknown end-to-end spec + ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG @@ -78,7 +79,7 @@ class CarInterface(object): stop_and_go = True if (candidate in CAR.RAV4H) else False ret.safetyParam = 73 ret.wheelbase = 2.65 - ret.steerRatio = 16.30 # 14.5 is spec end-to-end + ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] @@ -88,7 +89,7 @@ class CarInterface(object): stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 - ret.steerRatio = 17.8 + ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] @@ -213,7 +214,7 @@ class CarInterface(object): ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) + ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU) ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS) ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index b5b38bafd8..6c0cd006f6 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,8 +1,9 @@ #ifndef MODELDATA_H #define MODELDATA_H -#define MODEL_PATH_DISTANCE 100 +#define MODEL_PATH_DISTANCE 192 #define POLYFIT_DEGREE 4 +#define SPEED_PERCENTILES 10 typedef struct PathData { float points[MODEL_PATH_DISTANCE]; @@ -16,8 +17,12 @@ typedef struct LeadData { float dist; float prob; float std; + float rel_y; + float rel_y_std; float rel_v; float rel_v_std; + float rel_a; + float rel_a_std; } LeadData; typedef struct ModelData { @@ -25,6 +30,8 @@ typedef struct ModelData { PathData left_lane; PathData right_lane; LeadData lead; + LeadData lead_future; + float speed[SPEED_PERCENTILES]; } ModelData; #endif diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 7bbcf5fad1..723b06d921 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -184,6 +184,9 @@ int read_db_all(const char* params_path, std::map *par while ((de = readdir(d))) { if (!isalnum(de->d_name[0])) continue; std::string key = std::string(de->d_name); + + if (key == "AccessToken") continue; + std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str())); (*params)[key] = value; diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 5c21a9db87..1148fbd95b 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.1-release" +#define COMMA_VERSION "0.6.2-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b2892b49dc..11844556c2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -48,6 +48,10 @@ def events_to_bytes(events): ret.append(e.to_bytes()) return ret +def wait_for_can(logcan): + print("Waiting for CAN messages...") + while len(messaging.recv_one(logcan).can) == 0: + pass def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params): @@ -358,7 +362,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca "angleModelBias": 0., "gpsPlannerActive": sm['plan'].gpsPlannerActive, "vCurvature": sm['plan'].vCurvature, - "decelForTurn": sm['plan'].decelForTurn, + "decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, "cumLagMs": -rk.remaining * 1000., "startMonoTime": int(start_time * 1e9), "mapValid": sm['plan'].mapValid, @@ -412,7 +416,6 @@ def controlsd_thread(gctx=None): params = Params() - # Pub Sockets sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) @@ -427,15 +430,18 @@ def controlsd_thread(gctx=None): sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan']) logcan = messaging.sub_sock(service_list['can'].port) - CI, CP = get_car(logcan, sendcan) + + # wait for health and CAN packets + hw_type = messaging.recv_one(sm.sock['health']).health.hwType + is_panda_black = hw_type == log.HealthData.HwType.blackPanda + wait_for_can(logcan) + + CI, CP = get_car(logcan, sendcan, is_panda_black) logcan.close() # TODO: Use the logcan socket from above, but that will currenly break the tests can_sock = messaging.sub_sock(service_list['can'].port, timeout=100) - CC = car.CarControl.new_message() - AM = AlertManager() - car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive @@ -443,6 +449,13 @@ def controlsd_thread(gctx=None): if read_only: CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only + # Write CarParams for radard and boardd safety mode + params.put("CarParams", CP.to_bytes()) + params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") + + CC = car.CarControl.new_message() + AM = AlertManager() + startup_alert = get_startup_alert(car_recognized, controller_available) AM.add(sm.frame, startup_alert, False) @@ -456,10 +469,6 @@ def controlsd_thread(gctx=None): driver_status = DriverStatus() - # Write CarParams for radard and boardd safety mode - params.put("CarParams", CP.to_bytes()) - params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") - state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 @@ -473,6 +482,7 @@ def controlsd_thread(gctx=None): events_prev = [] sm['pathPlan'].sensorValid = True + sm['pathPlan'].posenetValid = True # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) @@ -498,6 +508,8 @@ def controlsd_thread(gctx=None): events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) + if not sm['pathPlan'].posenetValid: + events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['plan'].radarValid: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 0b0d5b1d54..90a92cb0f6 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -383,6 +383,13 @@ ALERTS = [ AlertStatus.critical, AlertSize.full, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.), + Alert( + "posenetInvalid", + "TAKE CONTROL IMMEDIATELY", + "Vision Failure: Check Camera View", + AlertStatus.critical, AlertSize.full, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.), + # Cancellation alerts causing immediate disabling Alert( "controlsFailed", @@ -540,6 +547,13 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + Alert( + "posenetInvalidNoEntry", + "openpilot Unavailable", + "Vision Failure: Check Camera View", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), + Alert( "controlsFailedNoEntry", "openpilot Unavailable", diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index d38a115f97..083d408bfd 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -10,6 +10,7 @@ _DISTRACTED_TIME = 7. _DISTRACTED_PRE_TIME = 4. _DISTRACTED_PROMPT_TIME = 2. # model output refers to center of cropped image, so need to apply the x displacement offset +_FACE_THRESHOLD = 0.4 _PITCH_WEIGHT = 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 _PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch @@ -23,16 +24,15 @@ MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts RESIZED_FOCAL = 320.0 H, W, FULL_W = 320, 160, 426 - -def head_orientation_from_descriptor(desc): +def head_orientation_from_descriptor(angles_desc, pos_desc): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right # TODO this should be calibrated - pitch_prnet = desc[0] - yaw_prnet = desc[1] - roll_prnet = desc[2] + pitch_prnet = angles_desc[0] + yaw_prnet = angles_desc[1] + roll_prnet = angles_desc[2] - face_pixel_position = ((desc[3] + .5)*W - W + FULL_W, (desc[4]+.5)*H) + face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL) pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL) @@ -96,24 +96,22 @@ class DriverStatus(): pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.) pitch_error *= _PITCH_WEIGHT metric = np.sqrt(yaw_error**2 + pitch_error**2) - #print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric + # TODO: do something with the eye states and combine them with head pose return 1 if metric > _METRIC_THRESHOLD else 0 def get_pose(self, driver_monitoring, params): + if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0: + return - self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.descriptor) - - # TODO: DM data should not be in a list if they are not homogeneous - if len(driver_monitoring.descriptor) > 6: - self.face_detected = driver_monitoring.descriptor[6] > 0. - else: - self.face_detected = True + self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition) + self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD self.driver_distracted = self._is_driver_distracted(self.pose) # first order filters self.driver_distraction_filter.update(self.driver_distracted) - self.variance_high = driver_monitoring.std > _STD_THRESHOLD + self.variance_high = False #driver_monitoring.std > _STD_THRESHOLD + self.variance_filter.update(self.variance_high) monitor_param_on_prev = self.monitor_param_on diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index cbd144edcd..190f7c843c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -126,6 +126,7 @@ class PathPlanner(object): plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) + plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid) self.plan.send(plan_send.to_bytes()) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index f5614eee72..2e01a8f4f9 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -3,6 +3,7 @@ import math import numpy as np from common.params import Params from common.numpy_fast import interp +from common.kalman.simple_kalman import KF1D import selfdrive.messaging as messaging from cereal import car @@ -15,7 +16,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc -NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS +MAX_SPEED = 255.0 LON_MPC_STEP = 0.2 # first step is 0.2s MAX_SPEED_ERROR = 2.0 @@ -37,6 +38,16 @@ _A_TOTAL_MAX_V = [1.5, 1.9, 3.2] _A_TOTAL_MAX_BP = [0., 20., 40.] +# Model speed kalman stuff +_MODEL_V_A = [[1.0, DT_PLAN], [0.0, 1.0]] +_MODEL_V_C = [1.0, 0] +# calculated with observation std of 2m/s and accel proc noise of 2m/s**2 +_MODEL_V_K = [[0.07068858], [0.04826294]] + +# 75th percentile +SPEED_PERCENTILE_IDX = 7 + + def calc_cruise_accel_limits(v_ego, following): a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) @@ -57,8 +68,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) - a_target[1] = min(a_target[1], a_x_allowed) - return a_target + return [a_target[0], min(a_target[1], a_x_allowed)] class Planner(object): @@ -79,16 +89,21 @@ class Planner(object): self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 + self.v_model = 0.0 + self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled + self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K) + self.model_v_kf_ready = False + self.params = Params() def choose_solution(self, v_cruise_setpoint, enabled): if enabled: - solutions = {'cruise': self.v_cruise} + solutions = {'cruise': self.v_cruise, 'model': self.v_model} if self.mpc1.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: @@ -108,10 +123,13 @@ class Planner(object): elif slowest == 'cruise': self.v_acc = self.v_cruise self.a_acc = self.a_cruise + elif slowest == 'model': + self.v_acc = self.v_model + self.a_acc = self.a_model self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) - def update(self, sm, CP, VM, PP, live_map_data): + def update(self, sm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo @@ -127,51 +145,43 @@ class Planner(object): enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 - v_speedlimit = NO_CURVATURE_SPEED - v_curvature = NO_CURVATURE_SPEED - - #map_age = cur_time - rcv_times['liveMapData'] - map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0 - - # Speed limit and curvature - set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None - if set_speed_limit_active and map_valid: - if live_map_data.liveMapData.speedLimitValid: - speed_limit = live_map_data.liveMapData.speedLimit - offset = float(self.params.get("SpeedLimitOffset")) - v_speedlimit = speed_limit + offset + if not self.model_v_kf_ready: + self.model_v_kf.x = [[v_ego],[0.0]] + self.model_v_kf_ready = True - if live_map_data.liveMapData.curvatureValid: - curvature = abs(live_map_data.liveMapData.curvature) - a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph - v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) - v_curvature = min(NO_CURVATURE_SPEED, v_curvature) + if len(sm['model'].speed): + self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX]) - decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) - v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit]) + if self.params.get("LimitSetSpeedNeural") == "1": + model_speed = self.model_v_kf.x[0][0] + else: + model_speed = MAX_SPEED # Calculate speed for normal cruise control if enabled: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning - accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration - accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) - accel_limits[0] = min(accel_limits[0], accel_limits[1]) - - # Change accel limits based on time remaining to turn - if decel_for_turn: - time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) - required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn) - accel_limits[0] = max(accel_limits[0], required_decel) + accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) + accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, - accel_limits[1], accel_limits[0], + accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1], jerk_limits[0], LON_MPC_STEP) + + # accel and jerk up limits are higher here to make model not limiting accel + # mainly done to prevent flickering of slowdown icon + self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start, + model_speed, + 2*accel_limits[1], 3*accel_limits[0], + 2*jerk_limits[1], jerk_limits[0], + LON_MPC_STEP) + # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: @@ -234,10 +244,6 @@ class Planner(object): plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource - plan_send.plan.vCurvature = v_curvature - plan_send.plan.decelForTurn = decel_for_turn - plan_send.plan.mapValid = map_valid - radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 7efb9f334d..6b3d768c7d 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,48 +1,35 @@ -import numpy as np - -from common.numpy_fast import clip, interp +from common.realtime import DT_MDL from common.kalman.simple_kalman import KF1D +from selfdrive.config import RADAR_TO_CENTER + +# the longer lead decels, the more likely it will keep decelerating +# TODO is this a good default? _LEAD_ACCEL_TAU = 1.5 -NO_FUSION_SCORE = 100 # bad default fusion score # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum -rate, ratev = 20., 20. # model and radar are both at 20Hz -ts = 1./rate -freq_v_lat = 0.2 # Hz -k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts) - -freq_a_lead = .5 # Hz -k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts) - # stationary qualification parameters -v_stationary_thr = 4. # objects moving below this speed are classified as stationary -v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes" v_ego_stationary = 4. # no stationary object flag below this speed # Lead Kalman Filter params -_VLEAD_A = [[1.0, ts], [0.0, 1.0]] +_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]] _VLEAD_C = [1.0, 0.0] #_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]]) #_VLEAD_R = 1e3 #_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]]) -_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]] - -RDR_TO_LDR = 2.7 +_VLEAD_K = [[0.1988689], [0.28555364]] class Track(object): def __init__(self): self.ekf = None - self.stationary = True self.initted = False - def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override): + def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured): if self.initted: # pylint: disable=access-member-before-definition - self.dPathPrev = self.dPath self.vLeadPrev = self.vLead self.vRelPrev = self.vRel @@ -52,9 +39,6 @@ class Track(object): self.vRel = v_rel # REL_SPEED self.measured = measured # measured or estimate - # compute distance to path - self.dPath = d_path - # computed velocity and accelerations self.vLead = self.vRel + v_ego_t_aligned @@ -64,21 +48,8 @@ class Track(object): self.cnt = 1 self.vision_cnt = 0 self.vision = False - self.aRel = 0. # nidec gives no information about this - self.vLat = 0. self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K) else: - # estimate acceleration - # TODO: use Kalman filter - a_rel_unfilt = (self.vRel - self.vRelPrev) / ts - a_rel_unfilt = clip(a_rel_unfilt, -10., 10.) - self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel - - # TODO: use Kalman filter - # neglect steer override cases as dPath is too noisy - v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts - self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat - self.kf.update(self.vLead) self.cnt += 1 @@ -86,33 +57,12 @@ class Track(object): self.vLeadK = float(self.kf.x[SPEED][0]) self.aLeadK = float(self.kf.x[ACCEL][0]) - if self.stationary: - # stationary objects can become non stationary, but not the other way around - self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr - self.oncoming = self.vLead < v_oncoming_thr - - self.vision_score = NO_FUSION_SCORE - # Learn if constant acceleration if abs(self.aLeadK) < 0.5: self.aLeadTau = _LEAD_ACCEL_TAU else: self.aLeadTau *= 0.9 - def update_vision_score(self, dist_to_vision, rel_speed_diff): - # rel speed is very hard to estimate from vision - if dist_to_vision < 4.0 and rel_speed_diff < 10.: - self.vision_score = dist_to_vision + rel_speed_diff - else: - self.vision_score = NO_FUSION_SCORE - - def update_vision_fusion(self): - # vision point is never stationary - # don't trust 1 or 2 fusions until model quality is much better - if self.vision_cnt >= 3: - self.vision = True - self.stationary = False - def get_key_for_cluster(self): # Weigh y higher since radar is inaccurate in this dimension return [self.dRel, self.yRel*2, self.vRel] @@ -171,89 +121,47 @@ class Cluster(object): def aLeadTau(self): return mean([t.aLeadTau for t in self.tracks]) - @property - def vision(self): - return any([t.vision for t in self.tracks]) - @property def measured(self): return any([t.measured for t in self.tracks]) - @property - def vision_cnt(self): - return max([t.vision_cnt for t in self.tracks]) - - @property - def stationary(self): - return all([t.stationary for t in self.tracks]) - - @property - def oncoming(self): - return all([t.oncoming for t in self.tracks]) - - def toRadarState(self): + def get_RadarState(self, model_prob=0.0): return { - "dRel": float(self.dRel) - RDR_TO_LDR, + "dRel": float(self.dRel), "yRel": float(self.yRel), "vRel": float(self.vRel), - "aRel": float(self.aRel), "vLead": float(self.vLead), - "dPath": float(self.dPath), - "vLat": float(self.vLat), "vLeadK": float(self.vLeadK), "aLeadK": float(self.aLeadK), "status": True, - "fcw": self.is_potential_fcw(), + "fcw": self.is_potential_fcw(model_prob), + "modelProb": model_prob, + "radar": True, "aLeadTau": float(self.aLeadTau) } + def get_RadarState_from_vision(self, lead_msg, v_ego): + return { + "dRel": float(lead_msg.dist - RADAR_TO_CENTER), + "yRel": float(lead_msg.relY), + "vRel": float(lead_msg.relVel), + "vLead": float(v_ego + lead_msg.relVel), + "vLeadK": float(v_ego + lead_msg.relVel), + "aLeadK": float(0), + "aLeadTau": _LEAD_ACCEL_TAU, + "fcw": False, + "modelProb": float(lead_msg.prob), + "radar": False, + "status": True + } + def __str__(self): - ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath) - if self.stationary: - ret += " stationary" - if self.vision: - ret += " vision" - if self.oncoming: - ret += " oncoming" - if self.vision_cnt > 0: - ret += " vision_cnt: %6.0f" % self.vision_cnt + ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK) return ret - def is_potential_lead(self, v_ego): - # predict cut-ins by extrapolating lateral speed by a lookahead time - # lookahead time depends on cut-in distance. more attentive for close cut-ins - # also, above 50 meters the predicted path isn't very reliable - - # the distance at which v_lat matters is higher at higher speed - lookahead_dist = 40. + v_ego/1.2 #40m at 0mph, ~70m at 80mph - - t_lookahead_v = [1., 0.] - t_lookahead_bp = [10., lookahead_dist] - - # average dist - d_path = self.dPath - - # lat_corr used to be gated on enabled, now always running - t_lookahead = interp(self.dRel, t_lookahead_bp, t_lookahead_v) - - # correct d_path for lookahead time, considering only cut-ins and no more than 1m impact. - lat_corr = clip(t_lookahead * self.vLat, -1., 1.) if self.measured else 0. - - # consider only cut-ins - d_path = clip(d_path + lat_corr, min(0., d_path), max(0.,d_path)) - - return abs(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 - return (self.dRel - lead_cluster.dRel) > 8. or abs(self.vRel - lead_cluster.vRel) > 1. - else: - return False + def potential_low_speed_lead(self, v_ego): + # stop for stuff in front of you and low speed, even without model confirmation + return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25 - 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 + def is_potential_fcw(self, model_prob): + return model_prob > .9 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 306dab05ba..9286ea0588 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -37,8 +37,6 @@ def plannerd_thread(): sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 - live_map_data = messaging.new_message() - live_map_data.init('liveMapData') while True: sm.update() @@ -46,9 +44,7 @@ def plannerd_thread(): if sm.updated['model']: PP.update(sm, CP, VM) if sm.updated['radarState']: - PL.update(sm, CP, VM, PP, live_map_data.liveMapData) - # elif socket is live_map_data_sock: - # live_map_data = msg + PL.update(sm, CP, VM, PP) def main(gctx=None): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index afb4ffd2bc..77e9febbcb 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -6,18 +6,13 @@ from collections import defaultdict, deque import selfdrive.messaging as messaging from selfdrive.services import service_list -from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset -from selfdrive.controls.lib.model_parser import ModelParser -from selfdrive.controls.lib.radar_helpers import Track, Cluster, \ - RDR_TO_LDR, NO_FUSION_SCORE - +from selfdrive.controls.lib.radar_helpers import Track, Cluster +from selfdrive.config import RADAR_TO_CENTER from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid -from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.swaglog import cloudlog from cereal import car from common.params import Params from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL -from common.kalman.ekf import EKF, SimpleSensor DEBUG = False @@ -26,106 +21,94 @@ DIMSV = 2 XV, SPEEDV = 0, 1 VISION_POINT = -1 -path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max - # Time-alignment rate = 1. / DT_MDL # model and radar are both at 20Hz v_len = 20 # how many speed data points to remember for t alignment with rdr data -class EKFV1D(EKF): - def __init__(self): - super(EKFV1D, self).__init__(False) - self.identity = numpy.matlib.identity(DIMSV) - self.state = np.matlib.zeros((DIMSV, 1)) - self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point - self.covar = self.identity * self.var_init - self.process_noise = np.matlib.diag([0.5, 1]) +def laplacian_cdf(x, mu, b): + b = np.max([b, 1e-4]) + return np.exp(-abs(x-mu)/b) + + +def match_vision_to_cluster(v_ego, lead, clusters): + # match vision point to best statistical cluster match + probs = [] + offset_vision_dist = lead.dist - RADAR_TO_CENTER + for c in clusters: + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std) + prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd) + prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd) + # This is isn't exactly right, but good heuristic + combined_prob = prob_d * prob_y * prob_v + probs.append(combined_prob) + idx = np.argmax(probs) + # if no 'sane' match is found return -1 + # stationary radar points can be false positives + dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2) + if dist_sane and vel_sane: + return idx + else: + return None + + +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): + # Determine leads, this is where the essential logic happens + if len(clusters) > 0 and ready and lead_msg.prob > .5: + lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters) + else: + lead_idx = None + + lead_dict = {'status': False} + if lead_idx is not None: + lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob) + elif (lead_idx is None) and ready and (lead_msg.prob > .5): + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + + if low_speed_override: + low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] + if len(low_speed_clusters) > 0: + lead_idx = np.argmin([c.dRel for c in low_speed_clusters]) + if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']): + lead_dict = low_speed_clusters[lead_idx].get_RadarState() + + return lead_dict - def calc_transfer_fun(self, dt): - tf = np.matlib.identity(DIMSV) - tf[XV, SPEEDV] = dt - tfj = tf - return tf, tfj class RadarD(object): - def __init__(self, VM, mocked): - self.VM = VM + def __init__(self, mocked): + self.current_time = 0 self.mocked = mocked - self.MP = ModelParser() self.tracks = defaultdict(dict) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 - self.steer_angle = 0. - self.steer_override = False - - # Kalman filter stuff: - self.ekfv = EKFV1D() - self.speedSensorV = SimpleSensor(XV, 1, 2) # v_ego self.v_ego = 0. self.v_ego_hist_t = deque([0], maxlen=v_len) self.v_ego_hist_v = deque([0], maxlen=v_len) self.v_ego_t_aligned = 0. + self.ready = False - def update(self, frame, delay, sm, rr): - ar_pts = {} - for pt in rr.points: - ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured] - - if sm.updated['liveParameters']: - self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) + def update(self, frame, delay, sm, rr, has_radar): + self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) if sm.updated['controlsState']: self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo - self.steer_angle = sm['controlsState'].angleSteers - self.steer_override = sm['controlsState'].steerOverride - self.v_ego_hist_v.append(self.v_ego) self.v_ego_hist_t.append(float(frame)/rate) - - self.last_controls_state_ts = sm.logMonoTime['controlsState'] - if sm.updated['model']: - self.last_md_ts = sm.logMonoTime['model'] - self.MP.update(self.v_ego, sm['model']) - - # run kalman filter only if prob is high enough - if self.MP.lead_prob > 0.7: - reading = self.speedSensorV.read(self.MP.lead_dist, covar=np.matrix(self.MP.lead_var)) - self.ekfv.update_scalar(reading) - self.ekfv.predict(DT_MDL) - - # When changing lanes the distance to the lead car can suddenly change, - # which makes the Kalman filter output large relative acceleration - if self.mocked and abs(self.MP.lead_dist - self.ekfv.state[XV]) > 2.0: - self.ekfv.state[XV] = self.MP.lead_dist - self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init])) - self.ekfv.state[SPEEDV] = 0. - - ar_pts[VISION_POINT] = (float(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])), - float(self.ekfv.state[SPEEDV]), False) - else: - self.ekfv.state[XV] = self.MP.lead_dist - self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init])) - self.ekfv.state[SPEEDV] = 0. - - if VISION_POINT in ar_pts: - del ar_pts[VISION_POINT] + self.ready = True - # *** compute the likely path_y *** - if (self.active and not self.steer_override) or self.mocked: - # use path from model (always when mocking as steering is too noisy) - path_y = np.polyval(self.MP.d_poly, path_x) - else: - # use path from steer, set angle_offset to 0 it does not only report the physical offset - path_y = calc_lookahead_offset(self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0] + ar_pts = {} + for pt in rr.points: + ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** remove missing points from meta data *** for ids in self.tracks.keys(): @@ -134,48 +117,21 @@ class RadarD(object): # *** compute the tracks *** for ids in ar_pts: - # ignore standalone vision point, unless we are mocking the radar - if ids == VISION_POINT and not self.mocked: - continue rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement cur_time = float(frame)/rate self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v) - d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) - # add sign - d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y)) - # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track() - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override) - - # allow the vision model to remove the stationary flag if distance and rel speed roughly match - if VISION_POINT in ar_pts: - fused_id = None - best_score = NO_FUSION_SCORE - for ids in self.tracks: - dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - self.tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - self.tracks[ids].yRel)) ** 2) - rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel) - self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) - if best_score > self.tracks[ids].vision_score: - fused_id = ids - best_score = self.tracks[ids].vision_score - - if fused_id is not None: - self.tracks[fused_id].vision_cnt += 1 - self.tracks[fused_id].update_vision_fusion() - - if DEBUG: - print("NEW CYCLE") - if VISION_POINT in ar_pts: - print("vision", ar_pts[VISION_POINT]) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3]) idens = list(self.tracks.keys()) track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens]) + # If we have multiple points, cluster them if len(track_pts) > 1: cluster_idxs = cluster_points_centroid(track_pts, 2.5) @@ -186,29 +142,12 @@ class RadarD(object): if clusters[cluster_i] is None: clusters[cluster_i] = Cluster() clusters[cluster_i].add(self.tracks[idens[idx]]) - elif len(track_pts) == 1: - # TODO: why do we need this? clusters = [Cluster()] clusters[0].add(self.tracks[idens[0]]) else: clusters = [] - if DEBUG: - for i in clusters: - print(i) - # *** extract the lead car *** - lead_clusters = [c for c in clusters - if c.is_potential_lead(self.v_ego)] - lead_clusters.sort(key=lambda x: x.dRel) - lead_len = len(lead_clusters) - - # *** extract the second lead from the whole set of leads *** - lead2_clusters = [c for c in lead_clusters - if c.is_potential_lead2(lead_clusters)] - lead2_clusters.sort(key=lambda x: x.dRel) - lead2_len = len(lead2_clusters) - # *** publish radarState *** dat = messaging.new_message() dat.init('radarState') @@ -217,18 +156,14 @@ class RadarD(object): dat.radarState.canMonoTimes = list(rr.canMonoTimes) dat.radarState.radarErrors = list(rr.errors) dat.radarState.controlsStateMonoTime = self.last_controls_state_ts - if lead_len > 0: - dat.radarState.leadOne = lead_clusters[0].toRadarState() - if lead2_len > 0: - dat.radarState.leadTwo = lead2_clusters[0].toRadarState() - else: - dat.radarState.leadTwo.status = False - else: - dat.radarState.leadOne.status = False + if has_radar: + dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) + dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) return dat -## fuses camera and radar data for best lead detection + +# fuses camera and radar data for best lead detection def radard_thread(gctx=None): set_realtime_priority(2) @@ -236,7 +171,6 @@ def radard_thread(gctx=None): cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" - VM = VehicleModel(CP) cloudlog.info("radard got CarParams") # import the radar from the fingerprint @@ -253,7 +187,9 @@ def radard_thread(gctx=None): liveTracks = messaging.pub_sock(service_list['liveTracks'].port) rk = Ratekeeper(rate, print_delay_threshold=None) - RD = RadarD(VM, mocked) + RD = RadarD(mocked) + + has_radar = not CP.radarOffCan while 1: can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) @@ -264,7 +200,7 @@ def radard_thread(gctx=None): sm.update(0) - dat = RD.update(rk.frame, RI.delay, sm, rr) + dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining*1000. radarState.send(dat.to_bytes()) @@ -275,29 +211,20 @@ def radard_thread(gctx=None): dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): - if DEBUG: - print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ - (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, - tracks[ids].dPath, tracks[ids].vLat, - tracks[ids].vLead, tracks[ids].vLeadK, - tracks[ids].aLeadK, - tracks[ids].stationary, - tracks[ids].measured)) dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), - "aRel": float(tracks[ids].aRel), - "stationary": bool(tracks[ids].stationary), - "oncoming": bool(tracks[ids].oncoming), } liveTracks.send(dat.to_bytes()) rk.monitor_time() + def main(gctx=None): radard_thread(gctx) + if __name__ == "__main__": main() diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index ae4b05a71a..df2c724899 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -5,6 +5,8 @@ #include #include +#include "cereal/gen/cpp/log.capnp.h" + #include "locationd_yawrate.h" @@ -42,6 +44,9 @@ void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odo double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2); double meas = camera_odometry.getRot()[2]; update_state(C_posenet, R, current_time, meas); + + auto trans = camera_odometry.getTrans(); + posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]); } void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) { @@ -65,10 +70,7 @@ Localizer::Localizer() { R_gyro = pow(0.05, 2.0); } -cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t msg_size) { - const kj::ArrayPtr view((const capnp::word*)msg_dat, msg_size); - capnp::FlatArrayMessageReader msg(view); - cereal::Event::Reader event = msg.getRoot(); +void Localizer::handle_log(cereal::Event::Reader event) { double current_time = event.getLogMonoTime() / 1.0e9; if (prev_update_time < 0) { @@ -89,8 +91,6 @@ cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t default: break; } - - return type; } @@ -101,8 +101,12 @@ extern "C" { } void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) { + const kj::ArrayPtr view((const capnp::word*)data, len); + capnp::FlatArrayMessageReader msg(view); + cereal::Event::Reader event = msg.getRoot(); + Localizer * loc = (Localizer*) localizer; - loc->handle_log(data, len); + loc->handle_log(event); } double localizer_get_yaw(void * localizer) { diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h index 323e87de4e..d5db91e790 100644 --- a/selfdrive/locationd/locationd_yawrate.h +++ b/selfdrive/locationd/locationd_yawrate.h @@ -25,10 +25,12 @@ public: Eigen::Vector2d x; double steering_angle = 0; double car_speed = 0; + double posenet_speed = 0; double prev_update_time = -1; double controls_state_time = -1; double sensor_data_time = -1; Localizer(); - cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size); + void handle_log(cereal::Event::Reader event); + }; diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index 9fc3ad625d..1766db311b 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -1,3 +1,4 @@ +#include #include #include #include @@ -63,6 +64,7 @@ int main(int argc, char *argv[]) { double sR = car_params.getSteerRatio(); double x = 1.0; double ao = 0.0; + double posenet_invalid_count = 0; if (result == 0){ auto str = std::string(value, value_sz); @@ -108,14 +110,25 @@ int main(int argc, char *argv[]) { assert(err == 0); err = zmq_msg_recv(&msg, polls[i].socket, 0); assert(err >= 0); + // 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)); - - auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size()); zmq_msg_close(&msg); - if (which == cereal::Event::CONTROLS_STATE){ + capnp::FlatArrayMessageReader capnp_msg(amsg); + cereal::Event::Reader event = capnp_msg.getRoot(); + + localizer.handle_log(event); + + auto which = event.which(); + if (which == cereal::Event::CAMERA_ODOMETRY){ + if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.5 * localizer.car_speed, 5.0)) { + posenet_invalid_count++; + } else { + posenet_invalid_count = 0; + } + } else if (which == cereal::Event::CONTROLS_STATE){ save_counter++; double yaw_rate = -localizer.x[0]; @@ -141,13 +154,14 @@ int main(int argc, char *argv[]) { live_params.setAngleOffsetAverage(angle_offset_average_degrees); live_params.setStiffnessFactor(learner.x); live_params.setSteerRatio(learner.sR); + live_params.setPosenetSpeed(localizer.posenet_speed); + live_params.setPosenetValid(posenet_invalid_count < 5); auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); } - // Save parameters every minute if (save_counter % 6000 == 0) { json11::Json json = json11::Json::object { diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py index e2a6913c0c..b29f591e7e 100755 --- a/selfdrive/locationd/test/test_params_learner.py +++ b/selfdrive/locationd/test/test_params_learner.py @@ -21,12 +21,12 @@ class TestParamsLearner(unittest.TestCase): # Setup vehicle model with wrong parameters VM_sim = VehicleModel(self.CP) x_target = 0.75 - sr_target = 14 + sr_target = self.CP.steerRatio - 0.5 ao_target = -1.0 VM_sim.update_params(x_target, sr_target) # Run simulation - times = np.arange(0, 10*3600, 0.01) + times = np.arange(0, 15*3600, 0.01) angle_offset = np.radians(ao_target) steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4e52f2beb4..6de644c336 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -133,6 +133,9 @@ unkillable_processes = ['visiond'] # processes to end with SIGINT instead of SIGTERM interrupt_processes = [] +# processes to end with SIGKILL instead of SIGTERM +kill_processes = ['sensord'] + persistent_processes = [ 'thermald', 'logmessaged', @@ -261,6 +264,8 @@ def kill_managed_process(name): if running[name].exitcode is None: if name in interrupt_processes: os.kill(running[name].pid, signal.SIGINT) + elif name in kill_processes: + os.kill(running[name].pid, signal.SIGKILL) else: running[name].terminate() @@ -368,7 +373,6 @@ def manager_thread(): logger_dead = False while 1: - # get health of board, log this in "thermal" msg = messaging.recv_sock(thermal_sock, wait=True) # uploader is gated based on the phone temperature @@ -566,6 +570,8 @@ def main(): params.put("LongitudinalControl", "0") if params.get("LimitSetSpeed") is None: params.put("LimitSetSpeed", "0") + if params.get("LimitSetSpeedNeural") is None: + params.put("LimitSetSpeedNeural", "0") # is this chffrplus? if os.getenv("PASSIVE") is not None: diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index 9d141c88d1..0c82a3187e 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -11,14 +11,14 @@ sensorEvents: [8003, true, 100., 100] # GPS data, also global timestamp gpsNMEA: [8004, true, 9.] # 9 msgs each sec # CPU+MEM+GPU+BAT temps -thermal: [8005, true, 1., 1] +thermal: [8005, true, 2., 1] # List(CanData), list of can messages can: [8006, true, 100.] controlsState: [8007, true, 100., 100] #liveEvent: [8008, true, 0.] model: [8009, true, 20.] features: [8010, true, 0.] -health: [8011, true, 1., 1] +health: [8011, true, 2., 1] radarState: [8012, true, 20.] #liveUI: [8014, true, 0.] encodeIdx: [8015, true, 20.] diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 551fbd0d9e..ea5ebdd9ee 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -1,3 +1,4 @@ +from collections import defaultdict from selfdrive.test.plant.maneuverplots import ManeuverPlot from selfdrive.test.plant.plant import Plant import numpy as np @@ -16,6 +17,7 @@ class Maneuver(object): self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration]) self.cruise_button_presses = kwargs.get("cruise_button_presses", []) + self.checks = kwargs.get("checks", []) self.duration = duration self.title = title @@ -28,6 +30,7 @@ class Maneuver(object): distance_lead = self.distance_lead ) + logs = defaultdict(list) last_controls_state = None plot = ManeuverPlot(self.title) @@ -42,20 +45,24 @@ class Maneuver(object): grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values) speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values) - distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade) - if controls_state: - last_controls_state = controls_state[-1] + # distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade) + log = plant.step(speed_lead, current_button, grade) - d_rel = distance_lead - distance if self.lead_relevancy else 200. - v_rel = speed_lead - speed if self.lead_relevancy else 0. + if log['controls_state_msgs']: + last_controls_state = log['controls_state_msgs'][-1] + + d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. + v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. + log['d_rel'] = d_rel + log['v_rel'] = v_rel if last_controls_state: # print(last_controls_state) #develop plots plot.add_data( time=plant.current_time(), - gas=gas, brake=brake, steer_torque=steer_torque, - distance=distance, speed=speed, acceleration=acceleration, + gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'], + distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'], up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd, uf_accel_cmd=last_controls_state.ufAccelCmd, d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead, @@ -63,10 +70,17 @@ class Maneuver(object): cruise_speed=last_controls_state.vCruise, jerk_factor=last_controls_state.jerkFactor, a_target=last_controls_state.aTarget, - fcw=fcw) - - print("maneuver end") + fcw=log['fcw']) - return (None, plot) + for k, v in log.items(): + logs[k].append(v) + valid = True + for check in self.checks: + c = check(logs) + if not c: + print check.__name__ + " not valid!" + valid = valid and c + print("maneuver end", valid) + return (plot, valid) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 930f83be87..3cd0d6b167 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -332,13 +332,15 @@ class Plant(object): live_parameters.init('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.sensorValid = True + live_parameters.liveParameters.posenetValid = True live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) driver_monitoring = messaging.new_message() driver_monitoring.init('driverMonitoring') - driver_monitoring.driverMonitoring.descriptor = [0.] * 7 + driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3 + driver_monitoring.driverMonitoring.facePosition = [0.] * 2 Plant.driverMonitoring.send(driver_monitoring.to_bytes()) health = messaging.new_message() @@ -364,9 +366,26 @@ class Plant(object): x.points = [0.0]*50 x.prob = 1.0 x.std = 1.0 + + if self.lead_relevancy: + d_rel = np.maximum(0., distance_lead - distance) + v_rel = v_lead - speed + prob = 1.0 + else: + d_rel = 200. + v_rel = 0. + prob = 0.0 + md.model.lead.dist = float(d_rel) - md.model.lead.prob = 1. - md.model.lead.std = 0.1 + md.model.lead.prob = prob + md.model.lead.relY = 0.0 + md.model.lead.relYStd = 1. + md.model.lead.relVel = float(v_rel) + md.model.lead.relVelStd = 1. + md.model.lead.relA = 0.0 + md.model.lead.relAStd = 10. + md.model.lead.std = 1.0 + cal.liveCalibration.calStatus = 1 cal.liveCalibration.calPerc = 100 # fake values? @@ -383,7 +402,17 @@ class Plant(object): self.distance_lead_prev = distance_lead self.rk.keep_time() - return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs) + return { + "distance": distance, + "speed": speed, + "acceleration": acceleration, + "distance_lead": distance_lead, + "brake": brake, + "gas": gas, + "steer_torque": steer_torque, + "fcw": fcw, + "controls_state_msgs": controls_state_msgs, + } # simple engage in standalone mode def plant_thread(rate=100): diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index 9806f5a190..1f30938ac1 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -22,22 +22,34 @@ def create_dir(path): except OSError: pass + +def check_no_collision(log): + return min(log['d_rel']) > 0 + +def check_fcw(log): + return any(log['fcw']) + +def check_engaged(log): + return log['controls_state_msgs'][-1][-1].active + maneuvers = [ Maneuver( 'while cruising at 40 mph, change cruise speed to 50mph', duration=30., initial_speed = 40. * CV.MPH_TO_MS, cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), - (CB.RES_ACCEL, 10.), (0, 10.1), - (CB.RES_ACCEL, 10.2), (0, 10.3)] + (CB.RES_ACCEL, 10.), (0, 10.1), + (CB.RES_ACCEL, 10.2), (0, 10.3)], + checks=[check_engaged], ), Maneuver( 'while cruising at 60 mph, change cruise speed to 50mph', duration=30., initial_speed=60. * CV.MPH_TO_MS, cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), - (CB.DECEL_SET, 10.), (0, 10.1), - (CB.DECEL_SET, 10.2), (0, 10.3)] + (CB.DECEL_SET, 10.), (0, 10.1), + (CB.DECEL_SET, 10.2), (0, 10.3)], + checks=[check_engaged], ), Maneuver( 'while cruising at 20mph, grade change +10%', @@ -45,7 +57,8 @@ maneuvers = [ initial_speed=20. * CV.MPH_TO_MS, cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], grade_values = [0., 0., 1.0], - grade_breakpoints = [0., 10., 11.] + grade_breakpoints = [0., 10., 11.], + checks=[check_engaged], ), Maneuver( 'while cruising at 20mph, grade change -10%', @@ -53,7 +66,8 @@ maneuvers = [ initial_speed=20. * CV.MPH_TO_MS, cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], grade_values = [0., 0., -1.0], - grade_breakpoints = [0., 10., 11.] + grade_breakpoints = [0., 10., 11.], + checks=[check_engaged], ), Maneuver( 'approaching a 40mph car while cruising at 60mph from 100m away', @@ -63,7 +77,8 @@ maneuvers = [ initial_distance_lead=100., speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS], speed_lead_breakpoints = [0., 100.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( 'approaching a 0mph car while cruising at 40mph from 150m away', @@ -73,7 +88,8 @@ maneuvers = [ initial_distance_lead=150., speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], speed_lead_breakpoints = [0., 100.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', @@ -83,7 +99,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values = [20., 20., 0.], speed_lead_breakpoints = [0., 15., 35.0], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', @@ -93,7 +110,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values = [20., 20., 0.], speed_lead_breakpoints = [0., 15., 25.0], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', @@ -103,7 +121,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values = [20., 20., 0.], speed_lead_breakpoints = [0., 15., 21.66], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + checks=[check_engaged, check_fcw], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2', @@ -113,7 +132,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values = [20., 20., 0.], speed_lead_breakpoints = [0., 15., 19.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + checks=[check_engaged, check_fcw], ), Maneuver( 'starting at 0mph, approaching a stopped car 100m away', @@ -122,9 +142,10 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=100., cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9)], + checks=[check_engaged, check_no_collision], ), Maneuver( "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s", @@ -135,8 +156,9 @@ maneuvers = [ speed_lead_values=[30.,30.,29.,31.,29.,31.,29.], speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)], + checks=[check_engaged, check_no_collision], ), Maneuver( "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel", @@ -147,8 +169,9 @@ maneuvers = [ speed_lead_values=[10., 0., 0., 10., 0.,10.], speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)], + checks=[check_engaged, check_no_collision], ), Maneuver( "green light: stopped behind lead car, lead car accelerates at 1.5 m/s", @@ -159,11 +182,12 @@ maneuvers = [ speed_lead_values=[0, 0 , 45], speed_lead_breakpoints=[0, 10., 40.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( "stop and go with 1m/s2 lead decel and accel, with full stops", @@ -175,7 +199,8 @@ maneuvers = [ speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + (CB.RES_ACCEL, 1.6), (0.0, 1.7)], + checks=[check_engaged, check_no_collision], ), Maneuver( "stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops", @@ -186,8 +211,9 @@ maneuvers = [ speed_lead_values=[10., 0., 0., 10., 0., 0.] , speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)], + checks=[check_engaged, check_no_collision], ), Maneuver( "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2", @@ -198,11 +224,12 @@ maneuvers = [ speed_lead_values=[20., 10.], speed_lead_breakpoints=[1., 11.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2", @@ -213,11 +240,12 @@ maneuvers = [ speed_lead_values=[20., 0.], speed_lead_breakpoints=[1., 11.], cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)], + checks=[check_engaged, check_no_collision], ), Maneuver( "fcw: traveling at 30 m/s and approaching lead traveling at 20m/s", @@ -227,7 +255,8 @@ maneuvers = [ initial_distance_lead=100., speed_lead_values=[20.], speed_lead_breakpoints=[1.], - cruise_button_presses = [] + cruise_button_presses = [], + checks=[check_fcw], ), Maneuver( "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2", @@ -237,7 +266,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values=[20., 0.], speed_lead_breakpoints=[3., 23.], - cruise_button_presses = [] + cruise_button_presses = [], + checks=[check_fcw], ), Maneuver( "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2", @@ -247,7 +277,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values=[20., 0.], speed_lead_breakpoints=[3., 9.6], - cruise_button_presses = [] + cruise_button_presses = [], + checks=[check_fcw], ), Maneuver( "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2", @@ -257,7 +288,8 @@ maneuvers = [ initial_distance_lead=35., speed_lead_values=[20., 0.], speed_lead_breakpoints=[3., 7.], - cruise_button_presses = [] + cruise_button_presses = [], + checks=[check_fcw], ) ] @@ -314,26 +346,30 @@ class LongitudinalControl(unittest.TestCase): def test_longitudinal_setup(self): pass -WORKERS = 8 def run_maneuver_worker(k): + man = maneuvers[k] output_dir = os.path.join(os.getcwd(), 'out/longitudinal') - for i, man in enumerate(maneuvers[k::WORKERS]): + + def run(self): + print(man.title) manager.start_managed_process('radard') manager.start_managed_process('controlsd') manager.start_managed_process('plannerd') - score, plot = man.evaluate() - plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2)) + plot, valid = man.evaluate() + plot.write_plot(output_dir, "maneuver" + str(k+1).zfill(2)) manager.kill_managed_process('radard') manager.kill_managed_process('controlsd') manager.kill_managed_process('plannerd') time.sleep(5) -for k in xrange(WORKERS): - setattr(LongitudinalControl, - "test_longitudinal_maneuvers_%d" % (k+1), - lambda self, k=k: run_maneuver_worker(k)) + self.assertTrue(valid) + + return run + +for k in range(len(maneuvers)): + setattr(LongitudinalControl, "test_longitudinal_maneuvers_%d" % (k+1), run_maneuver_worker(k)) if __name__ == "__main__": - unittest.main() + unittest.main(failfast=True) diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 9a29b96fec..b866521a92 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -8,7 +8,7 @@ import selfdrive.messaging as messaging from selfdrive.services import service_list from selfdrive.loggerd.config import get_available_percent from common.params import Params -from common.realtime import sec_since_boot +from common.realtime import sec_since_boot, DT_TRML from common.numpy_fast import clip from common.filter_simple import FirstOrderFilter @@ -138,8 +138,8 @@ def thermald_thread(): ignition_seen = False started_seen = False thermal_status = ThermalStatus.green - health_sock.RCVTIMEO = 1500 - current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.) + health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML) # 2x the expected health frequency + current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) health_prev = None # Make sure charging is enabled @@ -189,13 +189,13 @@ def thermald_thread(): if max_cpu_temp > 107. or bat_temp >= 63.: # onroad not allowed thermal_status = ThermalStatus.danger - elif max_comp_temp > 95. or bat_temp > 60.: + elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) - elif max_cpu_temp > 90.0: + elif max_cpu_temp > 87.5: # hysteresis between engage not allowed and uploader not allowed thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red) - elif max_cpu_temp > 85.0: + elif max_cpu_temp > 80.0: # uploader not allowed thermal_status = ThermalStatus.yellow elif max_cpu_temp > 75.0: @@ -266,7 +266,7 @@ def thermald_thread(): print(msg) # report to server once per minute - if (count%60) == 0: + if (count % int(60. / DT_TRML)) == 0: cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 6e24c9220b..15599414e2 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -126,8 +126,7 @@ typedef struct UIScene { float v_cruise; uint64_t v_cruise_update_ts; float v_ego; - float v_curvature; - bool decel_for_turn; + bool decel_for_model; float speedlimit; bool speedlimit_valid; @@ -1153,17 +1152,6 @@ static void ui_draw_vision_maxspeed(UIState *s) { nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL); } -#ifdef DEBUG_TURN - if (s->scene.decel_for_turn && s->scene.engaged){ - int v_curvature = s->scene.v_curvature * 2.2369363 + 0.5; - snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", v_curvature); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - nvgFontSize(s->vg, 25*2.5); - nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "TURN", NULL); - nvgFontSize(s->vg, 50*2.5); - nvgText(s->vg, 200 + viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL); - } -#endif } static void ui_draw_vision_speedlimit(UIState *s) { @@ -1294,7 +1282,7 @@ static void ui_draw_vision_event(UIState *s) { const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2))); const int viz_event_y = (box_y + (bdr_s*1.5)); const int viz_event_h = (header_h - (bdr_s*1.5)); - if (s->scene.decel_for_turn && s->scene.engaged && s->limit_set_speed) { + if (s->scene.decel_for_model && s->scene.engaged) { // draw winding road sign const int img_turn_size = 160*1.5; const int img_turn_x = viz_event_x-(img_turn_size/4); @@ -1643,8 +1631,7 @@ void handle_message(UIState *s, void *which) { s->scene.frontview = datad.rearViewCam; - s->scene.v_curvature = datad.vCurvature; - s->scene.decel_for_turn = datad.decelForTurn; + s->scene.decel_for_model = datad.decelForModel; if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) { char* error = NULL; diff --git a/selfdrive/visiond/models/commonmodel.c b/selfdrive/visiond/models/commonmodel.c index c2bb657bb5..33aabd1cee 100644 --- a/selfdrive/visiond/models/commonmodel.c +++ b/selfdrive/visiond/models/commonmodel.c @@ -97,39 +97,70 @@ static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, con for (int i=0; ioutput[i]); } printf("\n"); @@ -71,7 +75,8 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, //fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f); //fclose(f); //sleep(1); - //printf("done \n"); + //printf("%i \n",OUTPUT_SIZE); + //printf("%i \n",MDN_GROUP_SIZE); s->m->execute(net_input_buf); // net outputs @@ -79,6 +84,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2]; net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1]; net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2]; + net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS]; ModelData model = {0}; @@ -91,9 +97,9 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]); } - model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]); - model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]); - model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]); + model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]); + model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]); + model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]); model.path.prob = 1.; model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]); @@ -105,17 +111,59 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, const double max_dist = 140.0; const double max_rel_vel = 10.0; + // current lead int mdn_max_idx = 0; for (int i=1; i net_outputs.lead[mdn_max_idx*5 + 4]) { + if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) { + mdn_max_idx = i; + } + } + model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE]); + model.lead.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist; + model.lead.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist; + model.lead.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1]; + model.lead.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]); + model.lead.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel; + model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel; + model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3]; + model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]); + + // lead in 2s + mdn_max_idx = 0; + for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) { mdn_max_idx = i; } } - model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*5]); - model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist; - model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist; - model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel; - model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel; + model.lead_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]); + model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist; + model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist; + model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1]; + model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]); + model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel; + model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel; + model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3]; + model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]); + + + // get speed percentiles numbers represent 5th, 15th, ... 95th percentile + for (int i=0; i < SPEED_PERCENTILES; i++) { + model.speed[i] = ((float) SPEED_BUCKETS)/2.0; + } + float sum = 0; + for (int idx = 0; idx < SPEED_BUCKETS; idx++) { + sum += net_outputs.speed[idx]; + int idx_percentile = (sum + .05) * SPEED_PERCENTILES; + if (idx_percentile < SPEED_PERCENTILES ){ + model.speed[idx_percentile] = ((float)idx)/2.0; + } + } + // make sure no percentiles are skipped + for (int i=SPEED_PERCENTILES-1; i > 0; i--){ + if (model.speed[i-1] > model.speed[i]){ + model.speed[i-1] = model.speed[i]; + } + } return model; } diff --git a/selfdrive/visiond/models/monitoring.cc b/selfdrive/visiond/models/monitoring.cc index 7b151d0c41..9e874aa0e0 100644 --- a/selfdrive/visiond/models/monitoring.cc +++ b/selfdrive/visiond/models/monitoring.cc @@ -37,9 +37,11 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, s->m->execute(net_input_buf); MonitoringResult ret = {0}; - memcpy(ret.vs, s->output, sizeof(ret.vs)); - ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1]; - + memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); + memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position); + memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob); + memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob); + memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob); return ret; } diff --git a/selfdrive/visiond/models/monitoring.h b/selfdrive/visiond/models/monitoring.h index f9f4516bd8..80c65d0f0e 100644 --- a/selfdrive/visiond/models/monitoring.h +++ b/selfdrive/visiond/models/monitoring.h @@ -8,11 +8,18 @@ extern "C" { #endif -#define OUTPUT_SIZE 8 +#define OUTPUT_SIZE_DEPRECATED 8 +#define OUTPUT_SIZE 31 typedef struct MonitoringResult { - float vs[OUTPUT_SIZE - 1]; - float std; + float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1]; + float std_DEPRECATED; + + float face_orientation[3]; + float face_position[2]; + float face_prob; + float left_eye_prob; + float right_eye_prob; } MonitoringResult; typedef struct MonitoringState { diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index 530fd68f91..dd19912b3a 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -716,6 +716,8 @@ void* monitoring_thread(void *arg) { MonitoringResult res = monitoring_eval_frame(&s->monitoring, q, s->yuv_front_cl[buf_idx], s->yuv_front_width, s->yuv_front_height); + double t2 = millis_since_boot(); + // send driver monitoring packet { capnp::MallocMessageBuilder msg; @@ -725,17 +727,28 @@ void* monitoring_thread(void *arg) { auto framed = event.initDriverMonitoring(); framed.setFrameId(frame_data.frame_id); - kj::ArrayPtr descriptor_vs(&res.vs[0], ARRAYSIZE(res.vs)); - framed.setDescriptor(descriptor_vs); + // junk 0s from legacy model + //kj::ArrayPtr descriptor_DEPRECATED(&res.descriptor_DEPRECATED[0], ARRAYSIZE(res.descriptor_DEPRECATED)); + //framed.setDescriptor(descriptor_DEPRECATED); + //framed.setStd(res.std_DEPRECATED); + // why not use this junk space for reporting inference time instead + // framed.setStdDEPRECATED(static_cast(t2-t1)); + + kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); + kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); + framed.setFaceOrientation(face_orientation); + framed.setFacePosition(face_position); + framed.setFaceProb(res.face_prob); + framed.setLeftEyeProb(res.left_eye_prob); + framed.setRightEyeProb(res.right_eye_prob); - framed.setStd(res.std); auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); zmq_send(s->monitoring_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); } - double t2 = millis_since_boot(); + t2 = millis_since_boot(); //LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1;