openpilot v0.6.2 release

old-commit-hash: e90c41c576

Pipfile

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6f453d0dd767bc785836bf53d1ba3f4175188b4ca7b9fdcf8d146bb28c0cafb6
+oid sha256:9c487f33783e0fc4a6aee30c18c5e5b5391dca8cf360c531a69d46baa5f5a922
-size 2620
+size 2668

Pipfile.lock

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4e0fb1640f01f83f792d10e8957dc191edd1ff195a1188229730e08ee2198fdc
+oid sha256:795c35481c2671b9d4555fc684eb9a2b91e5cfdbc66be7a5650a7fef92b0d736
-size 155198
+size 157287

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)

apk/ai.comma.plus.offroad.apk

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:01a583ce80c2bf9e1a48c55b485cea03d6160358e0f5a528317040d30b9c0978
+oid sha256:3bf24d5dd606d364e339c8823a5b71b0a5271716b551a187ca8a8ea457a275d0
-size 17042125
+size 17042107

common/kalman/ekf.py

@@ -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

common/kalman/tests/test_ekf.py

@@ -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()

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],

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()

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):
+VIN_UNKNOWN = "0" * 17
-
-  # 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
-"""
-
 
 # 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(service_list['can'].port)
-  logcan = messaging.sub_sock(context, service_list['can'].port)
+  sendcan = messaging.pub_sock(service_list['sendcan'].port)
-  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
+  print get_vin(logcan, sendcan, 0)
-  time.sleep(1.)   # give time to sendcan socket to start
-
-  print get_vin(logcan, sendcan)

models/driving_model.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:46b4433f49e6d54ced1049e03aed650f6cb6de6d243ae64167945150935d7cd8
+oid sha256:32aef4d710c994e0c8a019b64eb2bd495f493be0dc0daa9f312f7dfeeb7f1568
-size 12039472
+size 14761531

models/monitoring_model.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f681047cee0e546c1438aecc99f3039456b4044f50e74745171a6aa0fab6503e
+oid sha256:eb047afc34b4b5b9888dc1dfc796d4f83fc695734decac5f6a6057a70e468623
-size 428211
+size 544528

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]) {
+  hw_type = (cereal::HealthData::HwType)(hw_query[0]);
-    LOGW("grey panda detected");
+  is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA);
-    is_grey_panda = true;
+  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) {

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_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)
+      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_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)
+      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_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)
+      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_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)
+      m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black)
       self.assertEqual(m_old, m)
 
 

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
+  params = Params()
-  cloudlog.warning("waiting for fingerprint...")
+  car_params = params.get("CarParams")
-  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 = ""
 
+  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
+      # need to independently try to fingerprint both bus 0 and 1 to work
-
+      # for the combo black_panda and honda_bosch. Ignore extended messages
-      # have we got a VIN query response?
+      # and 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.
       # 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 \
+      for b in candidate_cars:
-         can.address < 0x800 and can.address != 0x7e8:
+        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 = eliminate_incompatible_cars(can, candidate_cars)
+          candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
-
+
-    if can_seen_frame is None and can_seen:
+    # if we only have one car choice and the time since we got our first
-      can_seen_frame = frame
+    # message has elapsed, exit
-
+    for b in candidate_cars:
-    # if we only have one car choice and the time_fingerprint since we got our first
+      # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
-    # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
+      if only_toyota_left(candidate_cars[b]):
-    # broadcast immediately
+        frame_fingerprint = 100  # 1s
-    if len(candidate_cars) == 1 and can_seen_frame is not None:
+      if len(candidate_cars[b]) == 1:
-      time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1
+        if frame > frame_fingerprint:
-      if (frame - can_seen_frame) > (time_fingerprint * 100):
+          # fingerprint done
-        break
+          car_fingerprint = candidate_cars[b][0]
-
+
-    # bail if no cars left or we've been waiting for more than 2s since can_seen
+    # bail if no cars left or we've been waiting for more than 2s
-    elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200):
+    failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200
-      return None, finger, ""
+    succeeded = car_fingerprint is not None
-
+    done = failed or succeeded
-    # 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
 
     frame += 1
 
-  # only report vin if procedure is finished
+  cloudlog.warning("fingerprinted %s", car_fingerprint)
-  if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
+  return car_fingerprint, finger, vin
-    vin = "".join(vin_dat[3:])
-
-  cloudlog.warning("fingerprinted %s", candidate_cars[0])
-  cloudlog.warning("VIN %s", vin)
-  return candidate_cars[0], 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

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)
 

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
 

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},
   ],

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)
 

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
 

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:

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
+  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)
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
 
 class CarState(object):
   def __init__(self, CP):

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 = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
-  bus = 2 if car_fingerprint in HONDA_BOSCH else 0
   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 not in HONDA_BOSCH:
-  if car_fingerprint in HONDA_BOSCH:
-    bus = 2
-  else:
     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)

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.]

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),

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

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:

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

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']

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

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

selfdrive/common/params.cc

@@ -184,6 +184,9 @@ int read_db_all(const char* params_path, std::map<std::string, std::string> *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;

selfdrive/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.6.1-release"
+#define COMMA_VERSION "0.6.2-release"

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:

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",

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(angles_desc, pos_desc):
-def head_orientation_from_descriptor(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]
+  pitch_prnet = angles_desc[0]
-  yaw_prnet = desc[1]
+  yaw_prnet = angles_desc[1]
-  roll_prnet = desc[2]
+  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)
+    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
-    # 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.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

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())
 

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[0], min(a_target[1], a_x_allowed)]
-  return a_target
 
 
 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
+    if not self.model_v_kf_ready:
-    v_curvature = NO_CURVATURE_SPEED
+      self.model_v_kf.x = [[v_ego],[0.0]]
-
+      self.model_v_kf_ready = True
-    #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 live_map_data.liveMapData.curvatureValid:
+    if len(sm['model'].speed):
-        curvature = abs(live_map_data.liveMapData.curvature)
+      self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
-        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)
 
-    decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
+    if self.params.get("LimitSetSpeedNeural") == "1":
-    v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
+      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_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
-        accel_limits[0] = min(accel_limits[0], accel_limits[1])
+        accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[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)
 
       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)

selfdrive/controls/lib/radar_helpers.py

@@ -1,48 +1,35 @@
-import numpy as np
+from common.realtime import DT_MDL
-
-from common.numpy_fast import clip, interp
 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
-
 
 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 get_RadarState(self, model_prob=0.0):
-  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):
     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)
+    ret = "x: %4.1f  y: %4.1f  v: %4.1f  a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
-    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
     return ret
 
-  def is_potential_lead(self, v_ego):
+  def potential_low_speed_lead(self, v_ego):
-    # predict cut-ins by extrapolating lateral speed by a lookahead time
+    # stop for stuff in front of you and low speed, even without model confirmation
-    # lookahead time depends on cut-in distance. more attentive for close cut-ins
+    return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25
-    # 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 is_potential_fcw(self):
+  def is_potential_fcw(self, model_prob):
-    # is this cluster trustrable enough for triggering fcw?
+    return model_prob > .9
-    # fcw can trigger only on clusters that have been fused vision model for at least 20 frames
-    return self.vision_cnt >= 20

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)
+      PL.update(sm, CP, VM, PP)
-    # elif socket is live_map_data_sock:
-    #   live_map_data = msg
 
 
 def main(gctx=None):

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.radar_helpers import Track, Cluster
-from selfdrive.controls.lib.model_parser import ModelParser
+from selfdrive.config import RADAR_TO_CENTER
-from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
-                                                 RDR_TO_LDR, NO_FUSION_SCORE
-
 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):
+  def __init__(self, mocked):
-    self.VM = VM
+    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):
+  def update(self, frame, delay, sm, rr, has_radar):
-    ar_pts = {}
+    self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
-    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)
 
     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.ready = True
-      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]
 
-    # *** compute the likely path_y ***
+    ar_pts = {}
-    if (self.active and not self.steer_override) or self.mocked:
+    for pt in rr.points:
-      # use path from model (always when mocking as steering is too noisy)
+      ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
-      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]
 
     # *** 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)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3])
-
-    # 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])
 
     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()

selfdrive/locationd/locationd_yawrate.cc

@@ -5,6 +5,8 @@
 #include <capnp/serialize-packed.h>
 #include <eigen3/Eigen/Dense>
 
+#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) {
+void Localizer::handle_log(cereal::Event::Reader event) {
-  const kj::ArrayPtr<const capnp::word> view((const capnp::word*)msg_dat, msg_size);
-  capnp::FlatArrayMessageReader msg(view);
-  cereal::Event::Reader event = msg.getRoot<cereal::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<const capnp::word> view((const capnp::word*)data, len);
+    capnp::FlatArrayMessageReader msg(view);
+    cereal::Event::Reader event = msg.getRoot<cereal::Event>();
+
     Localizer * loc = (Localizer*) localizer;
-    loc->handle_log(data, len);
+    loc->handle_log(event);
   }
 
   double localizer_get_yaw(void * localizer) {

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);
+
 };

selfdrive/locationd/paramsd.cc

@@ -1,3 +1,4 @@
+#include <iostream>
 #include <czmq.h>
 #include <capnp/message.h>
 #include <capnp/serialize-packed.h>
@@ -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<capnp::word>((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<cereal::Event>();
+
+        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 {

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

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:

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.]

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)
+      # distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
-      if controls_state:
+      log = plant.step(speed_lead, current_button, grade)
-        last_controls_state = controls_state[-1]
 
-      d_rel = distance_lead - distance if self.lead_relevancy else 200.
+      if log['controls_state_msgs']:
-      v_rel = speed_lead - speed if self.lead_relevancy else 0.
+        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,
+          gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
-          distance=distance, speed=speed, acceleration=acceleration,
+          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)
+          fcw=log['fcw'])
-
-    print("maneuver end")
 
-    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)

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.prob = prob
-      md.model.lead.std = 0.1
+      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):

selfdrive/test/tests/plant/test_longitudinal.py

@@ -22,6 +22,16 @@ 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',
@@ -29,7 +39,8 @@ maneuvers = [
     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.2), (0, 10.3)],
+    checks=[check_engaged],
   ),
   Maneuver(
     'while cruising at 60 mph, change cruise speed to 50mph',
@@ -37,7 +48,8 @@ maneuvers = [
     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.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',
@@ -124,7 +144,8 @@ maneuvers = [
     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.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",
@@ -136,7 +157,8 @@ maneuvers = [
     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.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",
@@ -148,7 +170,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(
     "green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
@@ -163,7 +186,8 @@ maneuvers = [
                              (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, 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",
@@ -187,7 +212,8 @@ maneuvers = [
     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.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",
@@ -202,7 +228,8 @@ maneuvers = [
                              (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, 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",
@@ -217,7 +244,8 @@ maneuvers = [
                              (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, 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, valid = man.evaluate()
-    plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2))
+    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):
+    self.assertTrue(valid)
-  setattr(LongitudinalControl,
+
-    "test_longitudinal_maneuvers_%d" % (k+1),
+  return run
-    lambda self, k=k: run_maneuver_worker(k))
+
+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)

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
+  health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML)  # 2x the expected health frequency
-  current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
+  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),

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_model;
-  bool decel_for_turn;
 
   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_model = datad.decelForModel;
-    s->scene.decel_for_turn = datad.decelForTurn;
 
     if (datad.alertSound.str && datad.alertSound.str[0] != '\0' && strcmp(s->alert_type, datad.alertType.str) != 0) {
       char* error = NULL;

selfdrive/visiond/models/commonmodel.c

@@ -97,32 +97,59 @@ static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, con
   for (int i=0; i<POLYFIT_DEGREE; i++) {
     capn_set32(poly_ptr, i, capn_from_f32(data.poly[i]));
   }
-
+  if (getenv("DEBUG_MODEL")){
+    capn_list32 points_ptr = capn_new_list32(cs, MODEL_PATH_DISTANCE);
+    capn_list32 stds_ptr = capn_new_list32(cs, MODEL_PATH_DISTANCE);
+    for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
+      capn_set32(points_ptr, i, capn_from_f32(data.points[i]));
+      capn_set32(stds_ptr, i, capn_from_f32(data.stds[i]));
+    }
+    struct cereal_ModelData_PathData d = {
+      .points = points_ptr,
+      .stds = stds_ptr,
+      .prob = data.prob,
+      .std = data.std,
+      .poly = poly_ptr,
+    };
     cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
+    cereal_write_ModelData_PathData(&d, ret);
+    return ret;
+  } else {
     struct cereal_ModelData_PathData d = {
       .prob = data.prob,
       .std = data.std,
       .poly = poly_ptr,
     };
+    cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
     cereal_write_ModelData_PathData(&d, ret);
     return ret;
   }
+}
 
-void model_publish(void* sock, uint32_t frame_id,
-                   const mat3 transform, const ModelData data) {
-  struct capn rc;
-  capn_init_malloc(&rc);
-  struct capn_segment *cs = capn_root(&rc).seg;
 
+static cereal_ModelData_LeadData_ptr lead_to_cereal(struct capn_segment *cs, const LeadData data) {
   cereal_ModelData_LeadData_ptr leadp = cereal_new_ModelData_LeadData(cs);
   struct cereal_ModelData_LeadData leadd = (struct cereal_ModelData_LeadData){
-    .dist = data.lead.dist,
+    .dist = data.dist,
-    .prob = data.lead.prob,
+    .prob = data.prob,
-    .std = data.lead.std,
+    .std = data.std,
-    .relVel = data.lead.rel_v,
+    .relY = data.rel_y,
-    .relVelStd = data.lead.rel_v_std,
+    .relYStd = data.rel_y_std,
+    .relVel = data.rel_v,
+    .relVelStd = data.rel_v_std,
+    .relA = data.rel_a,
+    .relAStd = data.rel_a_std,
   };
   cereal_write_ModelData_LeadData(&leadd, leadp);
+  return leadp;
+}
+
+
+void model_publish(void* sock, uint32_t frame_id,
+                   const mat3 transform, const ModelData data) {
+  struct capn rc;
+  capn_init_malloc(&rc);
+  struct capn_segment *cs = capn_root(&rc).seg;
 
 
   capn_list32 input_transform_ptr = capn_new_list32(cs, 3*3);
@@ -130,6 +157,10 @@ void model_publish(void* sock, uint32_t frame_id,
     capn_set32(input_transform_ptr, i, capn_from_f32(transform.v[i]));
   }
 
+  capn_list32 speed_perc_ptr = capn_new_list32(cs, SPEED_PERCENTILES);
+  for (int i=0; i<SPEED_PERCENTILES; i++) {
+    capn_set32(speed_perc_ptr, i, capn_from_f32(data.speed[i]));
+  }
   cereal_ModelData_ModelSettings_ptr settingsp = cereal_new_ModelData_ModelSettings(cs);
   struct cereal_ModelData_ModelSettings settingsd = {
     .inputTransform = input_transform_ptr,
@@ -142,8 +173,10 @@ void model_publish(void* sock, uint32_t frame_id,
     .path = path_to_cereal(cs, data.path),
     .leftLane = path_to_cereal(cs, data.left_lane),
     .rightLane = path_to_cereal(cs, data.right_lane),
-    .lead = leadp,
+    .lead = lead_to_cereal(cs, data.lead),
+    .leadFuture = lead_to_cereal(cs, data.lead_future),
     .settings = settingsp,
+    .speed = speed_perc_ptr,
   };
   cereal_write_ModelData(&modeld, modelp);
 

selfdrive/visiond/models/driving.cc

@@ -22,9 +22,12 @@
   #define MODEL_NAME "driving_model_dlc"
 #endif
 
-#define OUTPUT_SIZE (200 + 2*201 + 26)
+#define LEAD_MDN_N 5 // probs for 5 groups
-#define LEAD_MDN_N 5
+#define MDN_VALS 4 // output xyva for each lead group
-
+#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
+#define MDN_GROUP_SIZE 11
+#define SPEED_BUCKETS 100
+#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + SPEED_BUCKETS)
 #ifdef TEMPORAL
   #define TEMPORAL_SIZE 512
 #else
@@ -60,6 +63,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
     float *left_lane;
     float *right_lane;
     float *lead;
+    float *speed;
   } net_outputs = {NULL};
 
   //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[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.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/2]);
+  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/2]);
+  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<LEAD_MDN_N; i++) {
-    if (net_outputs.lead[i*5 + 4] > 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<LEAD_MDN_N; i++) {
+    if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > 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_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
-  model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist;
+  model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
-  model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist;
+  model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
-  model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel;
+  model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
-  model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel;
+  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;
 }
 

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));
+  memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
-  ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1];
+  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;
 }
 

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 descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
-  float std;
+  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 {

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<const float> descriptor_vs(&res.vs[0], ARRAYSIZE(res.vs));
+        // junk 0s from legacy model
-        framed.setDescriptor(descriptor_vs);
+        //kj::ArrayPtr<const float> 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<float>(t2-t1));
+
+        kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
+        kj::ArrayPtr<const float> 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;