Merge branch 'devel' of https://github.com/commaai/openpilot into devel-en

This reverts commit d5242c5b82.

Pipfile

@@ -99,6 +99,8 @@ simplejson = "*"
 python-logstash-async = "*"
 pandas = "*"
 seaborn = "*"
+tensorflow-estimator = "==1.10.12"
+pyproj = "*"
 
 [packages]
 overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}

Pipfile.lock

@@ -1,7 +1,7 @@
 {
     "_meta": {
         "hash": {
-            "sha256": "89070d7d9478ac9e6ad2c9848aaf724cb7362a1de4af9f8fb8c40be5f37c043d"
+            "sha256": "43f96719ff9a1b7eae6c7a88f903c4f5a137c2b195e3ba148f32767827b98710"
         },
         "pipfile-spec": 6,
         "requires": {
@@ -656,12 +656,12 @@
         },
         "aenum": {
             "hashes": [
-                "sha256:058f0cfaf911899dc21b334362047df74ce989335dd8dff8e4be1a6313b15232",
-                "sha256:6af970173d9b4ac0384ad7d1cfe9523eeb9a3578793e1664090c13cb59df6469",
-                "sha256:80f14366578d84f6bccb0670259744cb3a7f2ab504480c306238a23cdd569457"
+                "sha256:0e3589654ef090784971f7778dcb74b08c9b4ef80b33267c00f82ddeedac179a",
+                "sha256:b12a7be3d89b270f266f8643aaa126404e5cdc0929bd6f09548b8eaed85e2aa1",
+                "sha256:e4dab068cbe00295bbc3660cd562221b008687d0f7a4c40fc2dd7a80002126a7"
             ],
             "index": "pypi",
-            "version": "==2.2.0"
+            "version": "==2.2.1"
         },
         "amqp": {
             "hashes": [
@@ -817,18 +817,18 @@
         },
         "boto3": {
             "hashes": [
-                "sha256:34a8ddb7247316be6ea94c7eeee41212312d250d99bf668fcd6748629b578622",
-                "sha256:71f3554cc69fa20be06cf20d6c9e0d8095d7c40695b48618676c3cd9a5ba0783"
+                "sha256:6a950bf98b22812896ea0f833a26d448acfdf43179f41f389d501af7a9fae328",
+                "sha256:cfbc062a76a7781af8e6a4ea26ebcafa3866872a8cceb05fdbf588c36e7848f0"
             ],
             "index": "pypi",
-            "version": "==1.9.189"
+            "version": "==1.9.195"
         },
         "botocore": {
             "hashes": [
-                "sha256:4febbf206d1dc8b8299aa211d8e382d5bf3f22097855b9f98d5e8c401ef8192b",
-                "sha256:b62ab3e4e98e075fc9e8e8fd4e8f5b92ebf311a6dc9f7578650938c7bc94e592"
+                "sha256:691627c2aeff0fcbd9237985717c28404a628181fd3e86b7be500bf2ee156007",
+                "sha256:c59e9981db9dfc54f0d22f731ca8de904760049a9c60d86dcedde84ae64ef4f0"
             ],
-            "version": "==1.12.189"
+            "version": "==1.12.195"
         },
         "celery": {
             "hashes": [
@@ -1734,7 +1734,7 @@
                 "sha256:23953d55076df038541e648a53676fb24980f7a1be290cdda21300b3bc21dfb0",
                 "sha256:552a91f381532e33cbd07c6a2655a21908088962bb8fa7239ecbcc6ad1140cc7"
             ],
-            "markers": "python_version == '2.7'",
+            "markers": "python_version == '2.6' or python_version == '2.7' or python_version == '3.0' or python_version == '3.1' or python_version == '3.2'",
             "version": "==1.5"
         },
         "mpld3": {
@@ -2264,6 +2264,32 @@
             "index": "pypi",
             "version": "==0.1.17"
         },
+        "pyproj": {
+            "hashes": [
+                "sha256:1303b343a1c4d83d1ac17494571d958335c0a4597d4aa623cd64df1f36fb18bf",
+                "sha256:1fd2f756f43ba762f5a1ae769c6cc9ae6f919996bf80c621f331e4a5d89ae74f",
+                "sha256:2b3f59b16cde23c0d95b1d9ba3dd1c0be3937d877df3b47a522d3c9a74d31668",
+                "sha256:2e5d8b892782db22c3d6ffcb272c8ac0a18995e22d45db762a893ec64cc039d6",
+                "sha256:512a59bc20587f88df78a69872ed8cb4ec7905415ba85b4de0af35601770fba1",
+                "sha256:5aa89b04855e439e2b96caf1f49fa2ab60e6e7c407034284d31d808abed0618a",
+                "sha256:75b4c5eba36060a00867e16e81755a44e93a95b0bbaa4d6ee40f10c330b62108",
+                "sha256:860fde3429f364767c88a8e7f14f330e3fdace9f438e0ef1b112108a44c1eee8",
+                "sha256:86886f21af819553990f0da9ad7737d52cd32076dd277db34e40cf805919e839",
+                "sha256:9417d46f2b5e6ba79bd6adea0aae9659501c8a0350a5a98c0d9cb1442d45b0b4",
+                "sha256:9bb81141715832bb8682dda3c2f8f1ad0f43cf8aec5db52e0992c16c7664e227",
+                "sha256:afe150d8f0b351df7a357cfce50a8a5c0a44bdfeec5e08075f873cfeddf63004",
+                "sha256:cc2e21c09245c3eb47014c3d4c58888cc8d48a168a03533f9ed3f8b2ee82a352",
+                "sha256:cefb4a1a5d51cd04c9dd31bb18047c2dafcb86bc33a0347746b5a0ef82c071d8",
+                "sha256:cfa2cd0ee23bcfc9f165b4f8e9c5b7ba829c7b49175fb73420544619148d91af",
+                "sha256:d3a78ce50e6c50e701d65929dc04ab751edaeae660455a9fef3c2231d609f8c1",
+                "sha256:d772ee53c8d316de1b7f68244ab654f864553bca177357f8787a65dc09816ae3",
+                "sha256:ddd99d6f2c813e44721c22039ddf333e4b95e8d0794b03d9962c94c01c823934",
+                "sha256:ec89e68a0cf210af0cc2724b5f8601d4b6809ff0f556e16efc8c955e79672f7a",
+                "sha256:f118762ed0abe8e988a20020241fe1e6133edb960d2665a44d6f48acc6d8e7c7"
+            ],
+            "index": "pypi",
+            "version": "==2.2.1"
+        },
         "pyrsistent": {
             "hashes": [
                 "sha256:50cffebc87ca91b9d4be2dcc2e479272bcb466b5a0487b6c271f7ddea6917e14"
@@ -2424,10 +2450,10 @@
         },
         "qtconsole": {
             "hashes": [
-                "sha256:4af84facdd6f00a6b9b2927255f717bb23ae4b7a20ba1d9ef0a5a5a8dbe01ae2",
-                "sha256:60d61d93f7d67ba2b265c6d599d413ffec21202fec999a952f658ff3a73d252b"
+                "sha256:6a85456af7a98b0f554d140922b7b6a219757b039adb2b95e847cf115eaa20ae",
+                "sha256:767eb9ec3f9943bc84270198b5ff95d2d86d68d6b57792fafa4df4fc6b16cd7c"
             ],
-            "version": "==4.5.1"
+            "version": "==4.5.2"
         },
         "redis": {
             "hashes": [
@@ -2685,9 +2711,10 @@
         },
         "tensorflow-estimator": {
             "hashes": [
-                "sha256:ca073f66063407a091d610ec1b22e39ea30248710198cc6f13769320bdbe3992"
+                "sha256:3e460f43682c7d789e5fe966630029558434d32502e632ee7f6703451258528c"
             ],
-            "version": "==1.14.0"
+            "index": "pypi",
+            "version": "==1.10.12"
         },
         "tensorflow-gpu": {
             "hashes": [

README.md

@@ -71,7 +71,7 @@ Supported Cars
 | Chrysler             | Pacifica Hybrid 2019     | Adaptive Cruise      | Yes     | Stock          | 0mph             | 39mph          | FCA               |
 | GMC<sup>3</sup>      | Acadia Denali 2018       | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Holden<sup>3</sup>   | Astra 2017               | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
-| Honda                | Accord 2018              | All                  | Yes     | Stock          | 0mph             | 3mph           | Bosch             |
+| Honda                | Accord 2018-19           | All                  | Yes     | Stock          | 0mph             | 3mph           | Bosch             |
 | Honda                | Civic Sedan/Coupe 2016-18| Honda Sensing        | Yes     | Yes            | 0mph             | 12mph          | Nidec             |
 | Honda                | Civic Sedan/Coupe 2019   | Honda Sensing        | Yes     | Stock          | 0mph             | 2mph           | Bosch             |
 | Honda                | Civic Hatchback 2017-19  | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |

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)

cereal/car.capnp

@@ -326,6 +326,7 @@ struct CarParams {
   lateralTuning :union {
     pid @26 :LateralPIDTuning;
     indi @27 :LateralINDITuning;
+    lqr @40 :LateralLQRTuning;
   }
 
   steerLimitAlert @28 :Bool;
@@ -341,6 +342,7 @@ struct CarParams {
   steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
   openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
   carVin @38 :Text; # VIN number queried during fingerprinting
+  isPandaBlack @39: Bool;
 
   struct LateralPIDTuning {
     kpBP @0 :List(Float32);
@@ -367,6 +369,20 @@ struct CarParams {
     actuatorEffectiveness @3 :Float32;
   }
 
+  struct LateralLQRTuning {
+    scale @0 :Float32;
+    ki @1 :Float32;
+    dcGain @2 :Float32;
+
+    # State space system
+    a @3 :List(Float32);
+    b @4 :List(Float32);
+    c @5 :List(Float32);
+
+    k @6 :List(Float32);  # LQR gain
+    l @7 :List(Float32);  # Kalman gain
+  }
+
 
   enum SafetyModel {
     # does NOT match board setting

cereal/log.capnp

@@ -301,7 +301,7 @@ struct HealthData {
   controlsAllowed @3 :Bool;
   gasInterceptorDetected @4 :Bool;
   startedSignalDetectedDeprecated @5 :Bool;
-  isGreyPanda @6 :Bool;
+  hasGps @6 :Bool;
   canSendErrs @7 :UInt32;
   canFwdErrs @8 :UInt32;
   gmlanSendErrs @9 :UInt32;
@@ -373,6 +373,8 @@ struct LiveCalibrationData {
   # view_frame_from_road_frame
   # ui's is inversed needs new
   extrinsicMatrix @4 :List(Float32);
+  # the direction of travel vector in device frame
+  rpyCalib @7 :List(Float32);
 }
 
 struct LiveTracks {
@@ -448,9 +450,12 @@ struct ControlsState @0x97ff69c53601abf1 {
   vCurvature @46 :Float32;
   decelForTurn @47 :Bool;
 
+  decelForModel @54 :Bool;
+
   lateralControlState :union {
     indiState @52 :LateralINDIState;
     pidState @53 :LateralPIDState;
+    lqrState @55 :LateralLQRState;
   }
 
   enum OpenpilotState @0xdbe58b96d2d1ac61 {
@@ -505,6 +510,13 @@ struct ControlsState @0x97ff69c53601abf1 {
     saturated @8 :Bool;
    }
 
+  struct LateralLQRState {
+    active @0 :Bool;
+    steerAngle @1 :Float32;
+    i @2 :Float32;
+    output @3 :Float32;
+   }
+
 }
 
 struct LiveEventData {
@@ -660,6 +672,7 @@ struct Plan {
     mpc1 @1;
     mpc2 @2;
     mpc3 @3;
+    model @4;
   }
 }
 
@@ -1661,8 +1674,13 @@ struct OrbKeyFrame {
 
 struct DriverMonitoring {
   frameId @0 :UInt32;
-  descriptor @1 :List(Float32);
-  std @2 :Float32;
+  descriptorDEPRECATED @1 :List(Float32);
+  stdDEPRECATED @2 :Float32;
+  faceOrientation @3 :List(Float32);
+  facePosition @4 :List(Float32);
+  faceProb @5 :Float32;
+  leftEyeProb @6 :Float32;
+  rightEyeProb @7 :Float32;
 }
 
 struct Boot {

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],
@@ -97,7 +99,14 @@ keys = {
   "DragonCachedFP": [TxType.PERSISTENT],
   "DragonCachedVIN": [TxType.PERSISTENT],
   "DragonAllowGas": [TxType.PERSISTENT],
-  "DragonBBUI": [TxType.PERSISTENT],
+  "DragonBBUI": [TxType.PERSISTENT], # deprecated
+  "DragonToyotaStockDSU": [TxType.PERSISTENT],
+  "DragonLatCtrl": [TxType.PERSISTENT],
+  "DragonUIEvent": [TxType.PERSISTENT],
+  "DragonUIMaxSpeed": [TxType.PERSISTENT],
+  "DragonUIFace": [TxType.PERSISTENT],
+  "DragonUIDev": [TxType.PERSISTENT],
+  "DragonUIDevMini": [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):
-
-  # works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
-  # Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
-  query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
-               [0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
-
-  cnts = [1, 2]  # Number of messages to wait for at each iteration
-  vin_valid = True
-
-  dat = []
-  for i in range(len(query_msg)):
-    cnt = 0
-    sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan'))
-    got_response = False
-    t_start = sec_since_boot()
-    while sec_since_boot() - t_start < 0.05 and not got_response:
-      for a in messaging.drain_sock(logcan):
-        for can in a.can:
-          if can.src == 0 and can.address == 0x7e8:
-            vin_valid = vin_valid and is_vin_response_valid(can.dat, i, cnt)
-            dat += can.dat[2:] if i == 0 else can.dat[1:]
-            cnt += 1
-            if cnt == cnts[i]:
-              got_response = True
-      time.sleep(0.01)
-
-  return "".join(dat[3:]) if vin_valid else ""
-
-"""
-if 'vin' not in gctx:
-  print "getting vin"
-  gctx['vin'] = query_vin()[3:]
-  print "got VIN %s" % (gctx['vin'],)
-  cloudlog.info("got VIN %s" % (gctx['vin'],))
-
-# *** determine platform based on VIN ****
-if vin.startswith("19UDE2F36G"):
-  print "ACURA ILX 2016"
-  self.civic = False
-else:
-  # TODO: add Honda check explicitly
-  print "HONDA CIVIC 2016"
-  self.civic = True
-
-# *** special case VIN of Acura test platform
-if vin == "19UDE2F36GA001322":
-  print "comma.ai test platform detected"
-  # it has a gas interceptor and a torque mod
-  self.torque_mod = True
-"""
-
+VIN_UNKNOWN = "0" * 17
 
 # sanity checks on response messages from vin query
 def is_vin_response_valid(can_dat, step, cnt):
@@ -84,12 +31,71 @@ def is_vin_response_valid(can_dat, step, cnt):
   return True
 
 
-if __name__ == "__main__":
-  import zmq
-  from selfdrive.services import service_list
-  context = zmq.Context()
-  logcan = messaging.sub_sock(context, service_list['can'].port)
-  sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
-  time.sleep(1.)   # give time to sendcan socket to start
+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]]
 
-  print get_vin(logcan, sendcan)
+    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__":
+  from selfdrive.services import service_list
+  logcan = messaging.sub_sock(service_list['can'].port)
+  sendcan = messaging.pub_sock(service_list['sendcan'].port)
+  print get_vin(logcan, sendcan, 0)

opendbc/chrysler_pacifica_2017_hybrid.dbc

@@ -262,7 +262,7 @@ BO_ 300 NEW_MSG_12C: 8 XXX
 
 BO_ 308 ACCEL_GAS_134: 8 XXX
  SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
- SG_ ACCEL_134 : 43|4@0+ (1,0) [0|15] "" XXX
+ SG_ ACCEL_134 : 46|7@0+ (1,0) [0|127] "" XXX
 
 BO_ 532 ENERGY_RELATED_214: 8 XXX
  SG_ NOISY_SLOWLY_DECREASING : 16|9@0+ (1,0) [0|255] "" XXX
@@ -413,6 +413,7 @@ CM_ SG_ 825 BEEP_339 "sent every 0.5s. 0050 is no beep. To beep send 4355 or 415
 CM_ SG_ 270 ELECTRIC_MOTOR "0x7fff indicates electric motor not in use";
 CM_ SG_ 291 ENERGY_GAIN_LOSS "unsure what this actually is";
 CM_ SG_ 291 ENERGY_SMOOTHER_CURVE "unusre what it is, but smoother";
+CM_ SG_ 308 ACCEL_134 "only set when human presses accel pedal";
 CM_ SG_ 532 NOISY_SLOWLY_DECREASING "perhaps battery but do not know";
 CM_ SG_ 816 TRACTION_OFF "set when traction off button is enabled";
 CM_ SG_ 816 TOGGLE_PARKSENSE "sending 3000071ec0ff9000 enables or disables parksense";

opendbc/hyundai_kia_generic.dbc

@@ -961,7 +961,7 @@ BO_ 64 DATC14: 8 DATC
  SG_ DATC_ADSDisp : 28|2@1+ (1.0,0.0) [0.0|3.0] ""  CLU
 
 BO_ 832 LKAS11: 8 LDWS_LKAS
- SG_ CF_Lkas_Icon : 0|2@1+ (1.0,0.0) [0.0|3.0] ""  CLU,IBOX,PSB
+ SG_ CF_Lkas_Bca_R : 0|2@1+ (1.0,0.0) [0.0|3.0] ""  CLU,IBOX,PSB
  SG_ CF_Lkas_LdwsSysState : 2|4@1+ (1.0,0.0) [0.0|15.0] ""  CLU,IBOX,PSB
  SG_ CF_Lkas_SysWarning : 6|4@1+ (1.0,0.0) [0.0|15.0] ""  BCM,CLU
  SG_ CF_Lkas_LdwsLHWarning : 10|2@1+ (1.0,0.0) [0.0|3.0] ""  BCM,CLU,PSB

panda/.circleci/config.yml

@@ -32,13 +32,6 @@ jobs:
       - store_artifacts:
           name: Store misra test output
           path: /tmp/misra/misra_output.txt
-      - store_artifacts:
-          name: Store cppcheck safety test output
-          path: /tmp/misra/cppcheck_safety_output.txt
-      - store_artifacts:
-          name: Store misra safety test output
-          path: /tmp/misra/misra_safety_output.txt
-
 
   build:
     machine:

panda/VERSION

@@ -1 +1 @@
-v1.4.1
+v1.4.2

panda/board/board.h

@@ -0,0 +1,61 @@
+// ///////////////////////////////////////////////////////////// //
+// Hardware abstraction layer for all different supported boards //
+// ///////////////////////////////////////////////////////////// //
+#include "board_declarations.h"
+#include "boards/common.h"
+
+// ///// Board definition and detection ///// //
+#include "drivers/harness.h"
+#ifdef PANDA
+  #include "boards/white.h"
+  #include "boards/grey.h"
+  #include "boards/black.h"
+#else
+  #include "boards/pedal.h"
+#endif
+
+void detect_board_type(void) {
+  #ifdef PANDA
+    // SPI lines floating: white (TODO: is this reliable?)
+    if((detect_with_pull(GPIOA, 4, PULL_DOWN)) || (detect_with_pull(GPIOA, 5, PULL_DOWN)) || (detect_with_pull(GPIOA, 6, PULL_DOWN)) || (detect_with_pull(GPIOA, 7, PULL_DOWN))){
+      hw_type = HW_TYPE_WHITE_PANDA;
+      current_board = &board_white;
+    } else if(detect_with_pull(GPIOA, 13, PULL_DOWN)) { // Rev AB deprecated, so no pullup means black. In REV C, A13 is pulled up to 5V with a 10K
+      hw_type = HW_TYPE_GREY_PANDA;
+      current_board = &board_grey;
+    } else {
+      hw_type = HW_TYPE_BLACK_PANDA;
+      current_board = &board_black;
+    }
+  #else
+    #ifdef PEDAL
+      hw_type = HW_TYPE_PEDAL;
+      current_board = &board_pedal;
+    #else 
+      hw_type = HW_TYPE_UNKNOWN;
+      puts("Hardware type is UNKNOWN!\n");
+    #endif
+  #endif
+}
+
+
+// ///// Configuration detection ///// //
+bool has_external_debug_serial = 0;
+bool is_entering_bootmode = 0;
+
+void detect_configuration(void) {
+  // detect if external serial debugging is present
+  has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);
+
+  #ifdef PANDA
+    // check if the ESP is trying to put me in boot mode
+    is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
+  #else
+    is_entering_bootmode = 0;
+  #endif
+}
+
+// ///// Board functions ///// //
+bool board_has_gps(void) {
+  return ((hw_type == HW_TYPE_GREY_PANDA) || (hw_type == HW_TYPE_BLACK_PANDA));
+}

panda/board/board_declarations.h

@@ -0,0 +1,57 @@
+// ******************** Prototypes ********************
+typedef void (*board_init)(void);
+typedef void (*board_enable_can_transciever)(uint8_t transciever, bool enabled);
+typedef void (*board_enable_can_transcievers)(bool enabled);
+typedef void (*board_set_led)(uint8_t color, bool enabled);
+typedef void (*board_set_usb_power_mode)(uint8_t mode);
+typedef void (*board_set_esp_gps_mode)(uint8_t mode);
+typedef void (*board_set_can_mode)(uint8_t mode);
+typedef void (*board_usb_power_mode_tick)(uint64_t tcnt);
+typedef bool (*board_check_ignition)(void);
+
+struct board {
+  const char *board_type;
+  const harness_configuration *harness_config;
+  board_init init;
+  board_enable_can_transciever enable_can_transciever;
+  board_enable_can_transcievers enable_can_transcievers;
+  board_set_led set_led;
+  board_set_usb_power_mode set_usb_power_mode;
+  board_set_esp_gps_mode set_esp_gps_mode;
+  board_set_can_mode set_can_mode;
+  board_usb_power_mode_tick usb_power_mode_tick;
+  board_check_ignition check_ignition;
+};
+
+// ******************* Definitions ********************
+// These should match the enum in cereal/log.capnp
+#define HW_TYPE_UNKNOWN 0U
+#define HW_TYPE_WHITE_PANDA 1U
+#define HW_TYPE_GREY_PANDA 2U
+#define HW_TYPE_BLACK_PANDA 3U
+#define HW_TYPE_PEDAL 4U
+
+// LED colors
+#define LED_RED 0U
+#define LED_GREEN 1U
+#define LED_BLUE 2U
+
+// USB power modes
+#define USB_POWER_NONE 0U
+#define USB_POWER_CLIENT 1U
+#define USB_POWER_CDP 2U
+#define USB_POWER_DCP 3U
+
+// ESP modes
+#define ESP_GPS_DISABLED 0U
+#define ESP_GPS_ENABLED 1U
+#define ESP_GPS_BOOTMODE 2U
+
+// CAN modes
+#define CAN_MODE_NORMAL 0U
+#define CAN_MODE_GMLAN_CAN2 1U
+#define CAN_MODE_GMLAN_CAN3 2U
+#define CAN_MODE_OBD_CAN2 3U
+
+// ********************* Globals **********************
+uint8_t usb_power_mode = USB_POWER_NONE;

panda/board/boards/black.h

@@ -0,0 +1,188 @@
+// ///////////////////// //
+// Black Panda + Harness //
+// ///////////////////// //
+
+void black_enable_can_transciever(uint8_t transciever, bool enabled) {
+  switch (transciever){
+    case 1U:
+      set_gpio_output(GPIOC, 1, !enabled);
+      break;
+    case 2U:
+      set_gpio_output(GPIOC, 13, !enabled);
+      break;
+    case 3U:
+      set_gpio_output(GPIOA, 0, !enabled);
+      break;
+    case 4U:
+      set_gpio_output(GPIOB, 10, !enabled);
+      break;
+    default:
+      puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+      break;
+  }
+}
+
+void black_enable_can_transcievers(bool enabled) {
+  for(uint8_t i=1; i<=4U; i++)
+    black_enable_can_transciever(i, enabled);
+}
+
+void black_set_led(uint8_t color, bool enabled) {
+  switch (color){
+    case LED_RED:
+      set_gpio_output(GPIOC, 9, !enabled);
+      break;
+     case LED_GREEN:
+      set_gpio_output(GPIOC, 7, !enabled);
+      break;
+    case LED_BLUE:
+      set_gpio_output(GPIOC, 6, !enabled);
+      break;  
+    default:
+      break;
+  }
+}
+
+void black_set_usb_power_mode(uint8_t mode){
+  usb_power_mode = mode;
+  puts("Trying to set USB power mode on black panda. This is not supported.\n");
+}
+
+void black_set_esp_gps_mode(uint8_t mode) {
+  switch (mode) {
+    case ESP_GPS_DISABLED:
+      // ESP OFF
+      set_gpio_output(GPIOC, 14, 0);
+      set_gpio_output(GPIOC, 5, 0);
+      break;
+    case ESP_GPS_ENABLED:
+      // ESP ON
+      set_gpio_output(GPIOC, 14, 1);
+      set_gpio_output(GPIOC, 5, 1);
+      break;
+    case ESP_GPS_BOOTMODE:
+      set_gpio_output(GPIOC, 14, 1);
+      set_gpio_output(GPIOC, 5, 0);
+      break;
+    default:
+      puts("Invalid ESP/GPS mode\n");
+      break;
+  }
+}
+
+void black_set_can_mode(uint8_t mode){
+  switch (mode) {
+    case CAN_MODE_NORMAL:
+    case CAN_MODE_OBD_CAN2:
+      if ((bool)(mode == CAN_MODE_NORMAL) != (bool)(car_harness_status == HARNESS_STATUS_NORMAL)) {
+        // B12,B13: disable OBD mode
+        set_gpio_mode(GPIOB, 12, MODE_INPUT);
+        set_gpio_mode(GPIOB, 13, MODE_INPUT);
+
+        // B5,B6: normal CAN2 mode
+        set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
+        set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+      } else {
+        // B5,B6: disable normal CAN2 mode
+        set_gpio_mode(GPIOB, 5, MODE_INPUT);
+        set_gpio_mode(GPIOB, 6, MODE_INPUT);
+
+        // B12,B13: OBD mode
+        set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
+        set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+      }      
+      break;
+    default:
+      puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
+      break;
+  }
+}
+
+void black_usb_power_mode_tick(uint64_t tcnt){
+  UNUSED(tcnt);
+  // Not applicable
+}
+
+bool black_check_ignition(void){
+  // ignition is checked through harness
+  return harness_check_ignition();
+}
+
+void black_init(void) {
+  common_init_gpio();
+
+  // A8,A15: normal CAN3 mode
+  set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
+  set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
+
+  // C0: OBD_SBU1 (orientation detection)
+  // C3: OBD_SBU2 (orientation detection)
+  set_gpio_mode(GPIOC, 0, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 3, MODE_ANALOG);
+
+  // C10: OBD_SBU1_RELAY (harness relay driving output)
+  // C11: OBD_SBU2_RELAY (harness relay driving output)
+  set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
+  set_gpio_mode(GPIOC, 11, MODE_OUTPUT);
+  set_gpio_output_type(GPIOC, 10, OUTPUT_TYPE_OPEN_DRAIN);
+  set_gpio_output_type(GPIOC, 11, OUTPUT_TYPE_OPEN_DRAIN);
+  set_gpio_output(GPIOC, 10, 1);
+  set_gpio_output(GPIOC, 11, 1);
+
+  // C8: FAN aka TIM3_CH3
+  set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
+
+  // C12: GPS load switch. Turn on permanently for now
+  set_gpio_output(GPIOC, 12, true);
+  //set_gpio_output(GPIOC, 12, false); //TODO: stupid inverted switch on prototype
+
+  // Initialize harness
+  harness_init();
+
+  // Enable CAN transcievers
+  black_enable_can_transcievers(true);
+
+  // Disable LEDs
+  black_set_led(LED_RED, false);
+  black_set_led(LED_GREEN, false);
+  black_set_led(LED_BLUE, false);
+
+  // Set normal CAN mode
+  black_set_can_mode(CAN_MODE_NORMAL);
+
+  // flip CAN0 and CAN2 if we are flipped
+  if (car_harness_status == HARNESS_STATUS_NORMAL) {
+    can_flip_buses(0, 2);
+  }
+
+  // init multiplexer
+  can_set_obd(car_harness_status, false);
+}
+
+const harness_configuration black_harness_config = {
+  .has_harness = true,
+  .GPIO_SBU1 = GPIOC,
+  .GPIO_SBU2 = GPIOC,
+  .GPIO_relay_normal = GPIOC,
+  .GPIO_relay_flipped = GPIOC,
+  .pin_SBU1 = 0,
+  .pin_SBU2 = 3,
+  .pin_relay_normal = 10,
+  .pin_relay_flipped = 11,
+  .adc_channel_SBU1 = 10,
+  .adc_channel_SBU2 = 13
+};
+
+const board board_black = {
+  .board_type = "Black",
+  .harness_config = &black_harness_config,
+  .init = black_init,
+  .enable_can_transciever = black_enable_can_transciever,
+  .enable_can_transcievers = black_enable_can_transcievers,
+  .set_led = black_set_led,
+  .set_usb_power_mode = black_set_usb_power_mode,
+  .set_esp_gps_mode = black_set_esp_gps_mode,
+  .set_can_mode = black_set_can_mode,
+  .usb_power_mode_tick = black_usb_power_mode_tick,
+  .check_ignition = black_check_ignition
+};

panda/board/boards/common.h

@@ -0,0 +1,82 @@
+#ifdef STM32F4
+  #include "stm32f4xx_hal_gpio_ex.h"
+#else
+  #include "stm32f2xx_hal_gpio_ex.h"
+#endif
+
+// Common GPIO initialization
+void common_init_gpio(void){
+  // TODO: Is this block actually doing something???
+  // pull low to hold ESP in reset??
+  // enable OTG out tied to ground
+  GPIOA->ODR = 0;
+  GPIOB->ODR = 0;
+  GPIOA->PUPDR = 0;
+  GPIOB->AFR[0] = 0;
+  GPIOB->AFR[1] = 0;
+
+  // C2: Voltage sense line
+  set_gpio_mode(GPIOC, 2, MODE_ANALOG);
+
+  // A11,A12: USB
+  set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS);
+  set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
+  GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
+
+  // A9,A10: USART 1 for talking to the ESP / GPS
+  set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
+  set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
+
+   // B8,B9: CAN 1
+  #ifdef STM32F4
+    set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
+    set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
+  #else
+    set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
+    set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
+  #endif
+}
+
+// Peripheral initialization
+void peripherals_init(void){
+  // enable GPIOB, UART2, CAN, USB clock
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
+
+  RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
+  RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
+  RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
+  #ifdef PANDA
+    RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
+  #endif
+  RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+  RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
+  #ifdef CAN3
+    RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
+  #endif
+  RCC->APB1ENR |= RCC_APB1ENR_DACEN;
+  RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;  // main counter
+  RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;  // slow loop and pedal
+  RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;  // gmlan_alt
+  //RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+  //RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;
+  RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
+  RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
+  //RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+  RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
+  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
+}
+
+// Detection with internal pullup
+#define PULL_EFFECTIVE_DELAY 10
+bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
+  set_gpio_mode(GPIO, pin, MODE_INPUT);
+  set_gpio_pullup(GPIO, pin, mode);
+  for (volatile int i=0; i<PULL_EFFECTIVE_DELAY; i++);
+  bool ret = get_gpio_input(GPIO, pin);
+  set_gpio_pullup(GPIO, pin, PULL_NONE);
+  return ret;
+}

panda/board/boards/grey.h

@@ -0,0 +1,18 @@
+// ////////// //
+// Grey Panda //
+// ////////// //
+
+// Most hardware functionality is similar to white panda
+const board board_grey = {
+  .board_type = "Grey",
+  .harness_config = &white_harness_config,
+  .init = white_init,
+  .enable_can_transciever = white_enable_can_transciever,
+  .enable_can_transcievers = white_enable_can_transcievers,
+  .set_led = white_set_led,
+  .set_usb_power_mode = white_set_usb_power_mode,
+  .set_esp_gps_mode = white_set_esp_gps_mode,
+  .set_can_mode = white_set_can_mode,
+  .usb_power_mode_tick = white_usb_power_mode_tick,
+  .check_ignition = white_check_ignition
+};

panda/board/boards/pedal.h

@@ -0,0 +1,96 @@
+// ///// //
+// Pedal //
+// ///// //
+
+void pedal_enable_can_transciever(uint8_t transciever, bool enabled) {
+  switch (transciever){
+    case 1:
+      set_gpio_output(GPIOB, 3, !enabled);
+      break;
+    default:
+      puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+      break;
+  }
+}
+
+void pedal_enable_can_transcievers(bool enabled) {
+  pedal_enable_can_transciever(1U, enabled);
+}
+
+void pedal_set_led(uint8_t color, bool enabled) {
+  switch (color){
+    case LED_RED:
+      set_gpio_output(GPIOB, 10, !enabled);
+      break;
+     case LED_GREEN:
+      set_gpio_output(GPIOB, 11, !enabled);
+      break;
+    default:
+      break;
+  }
+}
+
+void pedal_set_usb_power_mode(uint8_t mode){
+  usb_power_mode = mode;
+  puts("Trying to set USB power mode on pedal. This is not supported.\n");
+}
+
+void pedal_set_esp_gps_mode(uint8_t mode) {
+  UNUSED(mode);
+  puts("Trying to set ESP/GPS mode on pedal. This is not supported.\n");
+}
+
+void pedal_set_can_mode(uint8_t mode){
+  switch (mode) {
+    case CAN_MODE_NORMAL:
+      break;
+    default:
+      puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
+      break;
+  }
+}
+
+void pedal_usb_power_mode_tick(uint64_t tcnt){
+  UNUSED(tcnt);
+  // Not applicable
+}
+
+bool pedal_check_ignition(void){
+  // not supported on pedal
+  return false;
+}
+
+void pedal_init(void) {
+  common_init_gpio();
+
+  // C0, C1: Throttle inputs
+  set_gpio_mode(GPIOC, 0, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 1, MODE_ANALOG);
+  // DAC outputs on A4 and A5
+  //   apparently they don't need GPIO setup
+
+  // Enable transciever
+  pedal_enable_can_transcievers(true);
+
+  // Disable LEDs
+  pedal_set_led(LED_RED, false);
+  pedal_set_led(LED_GREEN, false);
+}
+
+const harness_configuration pedal_harness_config = {
+  .has_harness = false
+};
+
+const board board_pedal = {
+  .board_type = "Pedal",
+  .harness_config = &pedal_harness_config,
+  .init = pedal_init,
+  .enable_can_transciever = pedal_enable_can_transciever,
+  .enable_can_transcievers = pedal_enable_can_transcievers,
+  .set_led = pedal_set_led,
+  .set_usb_power_mode = pedal_set_usb_power_mode,
+  .set_esp_gps_mode = pedal_set_esp_gps_mode,
+  .set_can_mode = pedal_set_can_mode,
+  .usb_power_mode_tick = pedal_usb_power_mode_tick,
+  .check_ignition = pedal_check_ignition,
+};

panda/board/boards/white.h

@@ -0,0 +1,306 @@
+// /////////// //
+// White Panda //
+// /////////// //
+
+void white_enable_can_transciever(uint8_t transciever, bool enabled) {
+  switch (transciever){
+    case 1U:
+      set_gpio_output(GPIOC, 1, !enabled);
+      break;
+    case 2U:
+      set_gpio_output(GPIOC, 13, !enabled);
+      break;
+    case 3U:
+      set_gpio_output(GPIOA, 0, !enabled);
+      break;
+    default:
+      puts("Invalid CAN transciever ("); puth(transciever); puts("): enabling failed\n");
+      break;
+  }
+}
+
+void white_enable_can_transcievers(bool enabled) {
+  for(uint8_t i=1; i<=3U; i++)
+    white_enable_can_transciever(i, enabled);
+}
+
+void white_set_led(uint8_t color, bool enabled) {
+  switch (color){
+    case LED_RED:
+      set_gpio_output(GPIOC, 9, !enabled);
+      break;
+     case LED_GREEN:
+      set_gpio_output(GPIOC, 7, !enabled);
+      break;
+    case LED_BLUE:
+      set_gpio_output(GPIOC, 6, !enabled);
+      break;  
+    default:
+      break;
+  }
+}
+
+void white_set_usb_power_mode(uint8_t mode){
+  bool valid_mode = true;
+  switch (mode) {
+    case USB_POWER_CLIENT:
+      // B2,A13: set client mode
+      set_gpio_output(GPIOB, 2, 0);
+      set_gpio_output(GPIOA, 13, 1);
+      break;
+    case USB_POWER_CDP:
+      // B2,A13: set CDP mode
+      set_gpio_output(GPIOB, 2, 1);
+      set_gpio_output(GPIOA, 13, 1);
+      break;
+    case USB_POWER_DCP:
+      // B2,A13: set DCP mode on the charger (breaks USB!)
+      set_gpio_output(GPIOB, 2, 0);
+      set_gpio_output(GPIOA, 13, 0);
+      break;
+    default:
+      valid_mode = false;
+      puts("Invalid usb power mode\n");
+      break;
+  }
+
+  if (valid_mode) {
+    usb_power_mode = mode;
+  }
+}
+
+void white_set_esp_gps_mode(uint8_t mode) {
+  switch (mode) {
+    case ESP_GPS_DISABLED:
+      // ESP OFF
+      set_gpio_output(GPIOC, 14, 0);
+      set_gpio_output(GPIOC, 5, 0);
+      break;
+    case ESP_GPS_ENABLED:
+      // ESP ON
+      set_gpio_output(GPIOC, 14, 1);
+      set_gpio_output(GPIOC, 5, 1);
+      break;
+    case ESP_GPS_BOOTMODE:
+      set_gpio_output(GPIOC, 14, 1);
+      set_gpio_output(GPIOC, 5, 0);
+      break;
+    default:
+      puts("Invalid ESP/GPS mode\n");
+      break;
+  }
+}
+
+void white_set_can_mode(uint8_t mode){
+  switch (mode) {
+    case CAN_MODE_NORMAL:
+      // B12,B13: disable GMLAN mode
+      set_gpio_mode(GPIOB, 12, MODE_INPUT);
+      set_gpio_mode(GPIOB, 13, MODE_INPUT);
+
+      // B3,B4: disable GMLAN mode
+      set_gpio_mode(GPIOB, 3, MODE_INPUT);
+      set_gpio_mode(GPIOB, 4, MODE_INPUT);
+
+      // B5,B6: normal CAN2 mode
+      set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
+      set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+
+      // A8,A15: normal CAN3 mode
+      set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
+      set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
+      break;
+    case CAN_MODE_GMLAN_CAN2:
+      // B5,B6: disable CAN2 mode
+      set_gpio_mode(GPIOB, 5, MODE_INPUT);
+      set_gpio_mode(GPIOB, 6, MODE_INPUT);
+
+      // B3,B4: disable GMLAN mode
+      set_gpio_mode(GPIOB, 3, MODE_INPUT);
+      set_gpio_mode(GPIOB, 4, MODE_INPUT);
+
+      // B12,B13: GMLAN mode
+      set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
+      set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+
+      // A8,A15: normal CAN3 mode
+      set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
+      set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);   
+      break;
+    case CAN_MODE_GMLAN_CAN3:
+      // A8,A15: disable CAN3 mode
+      set_gpio_mode(GPIOA, 8, MODE_INPUT);
+      set_gpio_mode(GPIOA, 15, MODE_INPUT);
+
+      // B12,B13: disable GMLAN mode
+      set_gpio_mode(GPIOB, 12, MODE_INPUT);
+      set_gpio_mode(GPIOB, 13, MODE_INPUT);
+
+      // B3,B4: GMLAN mode
+      set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3);
+      set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3);
+
+      // B5,B6: normal CAN2 mode
+      set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
+      set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+      break;  
+    default:
+      puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
+      break;
+  }
+}
+
+uint64_t marker = 0;
+void white_usb_power_mode_tick(uint64_t tcnt){
+  #ifndef BOOTSTUB
+  #define CURRENT_THRESHOLD 0xF00U
+  #define CLICKS 5U // 5 seconds to switch modes
+
+  uint32_t current = adc_get(ADCCHAN_CURRENT);
+
+  // ~0x9a = 500 ma
+  // puth(current); puts("\n");
+
+  switch (usb_power_mode) {
+    case USB_POWER_CLIENT:
+      if ((tcnt - marker) >= CLICKS) {
+        if (!is_enumerated) {
+          puts("USBP: didn't enumerate, switching to CDP mode\n");
+          // switch to CDP
+          white_set_usb_power_mode(USB_POWER_CDP);
+          marker = tcnt;
+        }
+      }
+      // keep resetting the timer if it's enumerated
+      if (is_enumerated) {
+        marker = tcnt;
+      }
+      break;
+    case USB_POWER_CDP:
+      // On the EON, if we get into CDP mode we stay here. No need to go to DCP.
+      #ifndef EON
+        // been CLICKS clicks since we switched to CDP
+        if ((tcnt-marker) >= CLICKS) {
+          // measure current draw, if positive and no enumeration, switch to DCP
+          if (!is_enumerated && (current < CURRENT_THRESHOLD)) {
+            puts("USBP: no enumeration with current draw, switching to DCP mode\n");
+            white_set_usb_power_mode(USB_POWER_DCP);
+            marker = tcnt;
+          }
+        }
+        // keep resetting the timer if there's no current draw in CDP
+        if (current >= CURRENT_THRESHOLD) {
+          marker = tcnt;
+        }
+      #endif
+      break;
+    case USB_POWER_DCP:
+      // been at least CLICKS clicks since we switched to DCP
+      if ((tcnt-marker) >= CLICKS) {
+        // if no current draw, switch back to CDP
+        if (current >= CURRENT_THRESHOLD) {
+          puts("USBP: no current draw, switching back to CDP mode\n");
+          white_set_usb_power_mode(USB_POWER_CDP);
+          marker = tcnt;
+        }
+      }
+      // keep resetting the timer if there's current draw in DCP
+      if (current < CURRENT_THRESHOLD) {
+        marker = tcnt;
+      }
+      break;
+    default:
+      puts("USB power mode invalid\n");  // set_usb_power_mode prevents assigning invalid values
+      break;
+  }
+  #else
+  UNUSED(tcnt);
+  #endif
+}
+
+bool white_check_ignition(void){
+  // ignition is on PA1
+  return !get_gpio_input(GPIOA, 1);
+}
+
+void white_init(void) {
+  common_init_gpio();
+
+  // C3: current sense
+  set_gpio_mode(GPIOC, 3, MODE_ANALOG);
+
+  // A1: started_alt
+  set_gpio_pullup(GPIOA, 1, PULL_UP);
+
+  // A2, A3: USART 2 for debugging
+  set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2);
+  set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2);
+
+  // A4, A5, A6, A7: SPI
+  set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
+  set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
+  set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1);
+  set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
+
+  // Set USB power mode
+  white_set_usb_power_mode(USB_POWER_CLIENT);
+
+  // B12: GMLAN, ignition sense, pull up
+  set_gpio_pullup(GPIOB, 12, PULL_UP);
+
+  /* GMLAN mode pins:
+      M0(B15)  M1(B14)  mode
+      =======================
+      0        0        sleep
+      1        0        100kbit
+      0        1        high voltage wakeup
+      1        1        33kbit (normal)
+  */
+  set_gpio_output(GPIOB, 14, 1);
+  set_gpio_output(GPIOB, 15, 1);
+
+  // B7: K-line enable
+  set_gpio_output(GPIOB, 7, 1);
+
+  // C12, D2: Setup K-line (UART5)
+  set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5);
+  set_gpio_alternate(GPIOD, 2, GPIO_AF8_UART5);
+  set_gpio_pullup(GPIOD, 2, PULL_UP);
+
+  // L-line enable
+  set_gpio_output(GPIOA, 14, 1);
+
+  // C10, C11: L-Line setup (USART3)
+  set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3);
+  set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
+  set_gpio_pullup(GPIOC, 11, PULL_UP);
+
+  // Enable CAN transcievers
+  white_enable_can_transcievers(true);
+
+  // Disable LEDs
+  white_set_led(LED_RED, false);
+  white_set_led(LED_GREEN, false);
+  white_set_led(LED_BLUE, false);
+
+  // Set normal CAN mode
+  white_set_can_mode(CAN_MODE_NORMAL);
+}
+
+const harness_configuration white_harness_config = {
+  .has_harness = false
+};
+
+const board board_white = {
+  .board_type = "White",
+  .harness_config = &white_harness_config,
+  .init = white_init,
+  .enable_can_transciever = white_enable_can_transciever,
+  .enable_can_transcievers = white_enable_can_transcievers,
+  .set_led = white_set_led,
+  .set_usb_power_mode = white_set_usb_power_mode,
+  .set_esp_gps_mode = white_set_esp_gps_mode,
+  .set_can_mode = white_set_can_mode,
+  .usb_power_mode_tick = white_usb_power_mode_tick,
+  .check_ignition = white_check_ignition
+};

panda/board/bootstub.c

@@ -12,20 +12,29 @@
   #include "stm32f2xx_hal_gpio_ex.h"
 #endif
 
-// default since there's no serial
-void puts(const char *a) {
-  UNUSED(a);
-}
+// ******************** Prototypes ********************
+void puts(const char *a){ UNUSED(a); }
+void puth(unsigned int i){ UNUSED(i); }
+void puth2(unsigned int i){ UNUSED(i); }
+typedef struct board board;
+typedef struct harness_configuration harness_configuration;
+// No CAN support on bootloader
+void can_flip_buses(uint8_t bus1, uint8_t bus2){UNUSED(bus1); UNUSED(bus2);}
+void can_set_obd(int harness_orientation, bool obd){UNUSED(harness_orientation); UNUSED(obd);}
 
-void puth(unsigned int i) {
-  UNUSED(i);
-}
+// ********************* Globals **********************
+int hw_type = 0;
+const board *current_board;
 
+// ********************* Includes *********************
 #include "libc.h"
 #include "provision.h"
 
 #include "drivers/clock.h"
 #include "drivers/llgpio.h"
+
+#include "board.h"
+
 #include "gpio.h"
 
 #include "drivers/spi.h"
@@ -56,11 +65,10 @@ extern void *_app_start[];
 int main(void) {
   __disable_irq();
   clock_init();
-  detect();
+  detect_configuration();
+  detect_board_type();
 
-  if (revision == PANDA_REV_C) {
-    set_usb_power_mode(USB_POWER_CLIENT);
-  }
+  current_board->set_usb_power_mode(USB_POWER_CLIENT);
 
   if (enter_bootloader_mode == ENTER_SOFTLOADER_MAGIC) {
     enter_bootloader_mode = 0;

panda/board/build.mk

@@ -68,7 +68,7 @@ obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o
 	$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin
 	SETLEN=1 ../crypto/sign.py obj/code.bin $@ $(CERT)
 	@BINSIZE=$$(du -b "obj/$(PROJ_NAME).bin" | cut -f 1) ; \
-	if [ $$BINSIZE -ge 32768 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi;
+	if [ $$BINSIZE -ge 49152 ]; then echo "ERROR obj/$(PROJ_NAME).bin is too big!"; exit 1; fi;
 
 obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o
 	$(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^

panda/board/drivers/can.h

@@ -33,7 +33,6 @@ bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
 // end API
 
 #define ALL_CAN_SILENT 0xFF
-#define ALL_CAN_BUT_MAIN_SILENT 0xFE
 #define ALL_CAN_LIVE 0
 
 int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_SILENT;
@@ -149,7 +148,6 @@ void can_set_speed(uint8_t can_number) {
 void can_init(uint8_t can_number) {
   if (can_number != 0xffU) {
     CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
-    set_can_enable(CAN, 1);
     can_set_speed(can_number);
 
     llcan_init(CAN);
@@ -165,45 +163,86 @@ void can_init_all(void) {
   }
 }
 
-void can_set_gmlan(uint8_t bus) {
+void can_flip_buses(uint8_t bus1, uint8_t bus2){
+  bus_lookup[bus1] = bus2;
+  bus_lookup[bus2] = bus1;
+  can_num_lookup[bus1] = bus2;
+  can_num_lookup[bus2] = bus1;
+}
 
-  // first, disable GMLAN on prev bus
-  uint8_t prev_bus = can_num_lookup[3];
-  if (bus != prev_bus) {
-    switch (prev_bus) {
+// TODO: Cleanup with new abstraction
+void can_set_gmlan(uint8_t bus) {
+  if(hw_type != HW_TYPE_BLACK_PANDA){
+    // first, disable GMLAN on prev bus
+    uint8_t prev_bus = can_num_lookup[3];
+    if (bus != prev_bus) {
+      switch (prev_bus) {
+        case 1:
+        case 2:
+          puts("Disable GMLAN on CAN");
+          puth(prev_bus + 1U);
+          puts("\n");
+          current_board->set_can_mode(CAN_MODE_NORMAL);
+          bus_lookup[prev_bus] = prev_bus;
+          can_num_lookup[prev_bus] = prev_bus;
+          can_num_lookup[3] = -1;
+          can_init(prev_bus);
+          break;
+        default:
+          // GMLAN was not set on either BUS 1 or 2
+          break;
+      }
+    }
+
+    // now enable GMLAN on the new bus
+    switch (bus) {
       case 1:
       case 2:
-        puts("Disable GMLAN on CAN");
-        puth(prev_bus + 1U);
+        puts("Enable GMLAN on CAN");
+        puth(bus + 1U);
         puts("\n");
-        set_can_mode(prev_bus, 0);
-        bus_lookup[prev_bus] = prev_bus;
-        can_num_lookup[prev_bus] = prev_bus;
-        can_num_lookup[3] = -1;
-        can_init(prev_bus);
+        current_board->set_can_mode((bus == 1U) ? CAN_MODE_GMLAN_CAN2 : CAN_MODE_GMLAN_CAN3);
+        bus_lookup[bus] = 3;
+        can_num_lookup[bus] = -1;
+        can_num_lookup[3] = bus;
+        can_init(bus);
+        break;
+      case 0xFF:  //-1 unsigned
         break;
       default:
-        // GMLAN was not set on either BUS 1 or 2
+        puts("GMLAN can only be set on CAN2 or CAN3\n");
         break;
     }
+  } else {
+    puts("GMLAN not available on black panda\n");
   }
+}
 
-  // now enable GMLAN on the new bus
-  switch (bus) {
-    case 1:
-    case 2:
-      puts("Enable GMLAN on CAN");
-      puth(bus + 1U);
-      puts("\n");
-      set_can_mode(bus, 1);
-      bus_lookup[bus] = 3;
-      can_num_lookup[bus] = -1;
-      can_num_lookup[3] = bus;
-      can_init(bus);
-      break;
-    default:
-      puts("GMLAN can only be set on CAN2 or CAN3");
-      break;
+// TODO: remove
+void can_set_obd(uint8_t harness_orientation, bool obd){
+  if(obd){
+    puts("setting CAN2 to be OBD\n");
+  } else {
+    puts("setting CAN2 to be normal\n");
+  }
+  if(hw_type == HW_TYPE_BLACK_PANDA){
+    if(obd != (bool)(harness_orientation == HARNESS_STATUS_NORMAL)){
+        // B5,B6: disable normal mode
+        set_gpio_mode(GPIOB, 5, MODE_INPUT);
+        set_gpio_mode(GPIOB, 6, MODE_INPUT);
+        // B12,B13: CAN2 mode
+        set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
+        set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
+    } else {
+        // B5,B6: CAN2 mode
+        set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
+        set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
+        // B12,B13: disable normal mode
+        set_gpio_mode(GPIOB, 12, MODE_INPUT);
+        set_gpio_mode(GPIOB, 13, MODE_INPUT);
+    }
+  } else {
+    puts("OBD CAN not available on non-black panda\n");
   }
 }
 
@@ -326,7 +365,7 @@ void can_rx(uint8_t can_number) {
 
     safety_rx_hook(&to_push);
 
-    set_led(LED_BLUE, 1);
+    current_board->set_led(LED_BLUE, true);
     can_send_errs += !can_push(&can_rx_q, &to_push);
 
     // next

panda/board/drivers/harness.h

@@ -0,0 +1,130 @@
+uint8_t car_harness_status = 0U;
+#define HARNESS_STATUS_NC 0U
+#define HARNESS_STATUS_NORMAL 1U
+#define HARNESS_STATUS_FLIPPED 2U
+
+// Threshold voltage (mV) for either of the SBUs to be below before deciding harness is connected
+#define HARNESS_CONNECTED_THRESHOLD 2500U
+
+struct harness_configuration {
+  const bool has_harness;
+  GPIO_TypeDef *GPIO_SBU1;  
+  GPIO_TypeDef *GPIO_SBU2;
+  GPIO_TypeDef *GPIO_relay_normal;
+  GPIO_TypeDef *GPIO_relay_flipped;
+  uint8_t pin_SBU1;
+  uint8_t pin_SBU2;
+  uint8_t pin_relay_normal;
+  uint8_t pin_relay_flipped;
+  uint8_t adc_channel_SBU1;
+  uint8_t adc_channel_SBU2;
+};
+
+// this function will be the API for tici
+void set_intercept_relay(bool intercept) {
+  if (car_harness_status != HARNESS_STATUS_NC) {
+    if (intercept) {
+      puts("switching harness to intercept (relay on)\n");
+    } else {
+      puts("switching harness to passthrough (relay off)\n");
+    }
+
+    if(car_harness_status == HARNESS_STATUS_NORMAL){
+      set_gpio_output(current_board->harness_config->GPIO_relay_normal, current_board->harness_config->pin_relay_normal, !intercept);
+    } else {
+      set_gpio_output(current_board->harness_config->GPIO_relay_flipped, current_board->harness_config->pin_relay_flipped, !intercept);
+    }
+  }
+}
+
+bool harness_check_ignition(void) {
+  bool ret = false;
+  switch(car_harness_status){
+    case HARNESS_STATUS_NORMAL:
+      ret = !get_gpio_input(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2);
+      break;
+    case HARNESS_STATUS_FLIPPED:
+      ret = !get_gpio_input(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1);
+      break;
+    default:
+      break;
+  }
+  return ret;
+}
+
+// TODO: refactor to use harness config
+void harness_setup_ignition_interrupts(void){
+  if(car_harness_status == HARNESS_STATUS_NORMAL){
+    SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI3_PC;
+    EXTI->IMR |= (1U << 3);
+    EXTI->RTSR |= (1U << 3);
+    EXTI->FTSR |= (1U << 3);
+    puts("setup interrupts: normal\n");
+  } else if(car_harness_status == HARNESS_STATUS_FLIPPED) {
+    SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI0_PC;
+    EXTI->IMR |= (1U << 0);
+    EXTI->RTSR |= (1U << 0);
+    EXTI->FTSR |= (1U << 0);
+    NVIC_EnableIRQ(EXTI1_IRQn);
+    puts("setup interrupts: flipped\n");
+  } else {
+    puts("tried to setup ignition interrupts without harness connected\n");
+  }
+  NVIC_EnableIRQ(EXTI0_IRQn);
+  NVIC_EnableIRQ(EXTI3_IRQn);
+}
+
+uint8_t harness_detect_orientation(void) {
+  uint8_t ret = HARNESS_STATUS_NC;
+
+  #ifndef BOOTSTUB
+  uint32_t sbu1_voltage = adc_get(current_board->harness_config->adc_channel_SBU1);
+  uint32_t sbu2_voltage = adc_get(current_board->harness_config->adc_channel_SBU2);
+
+  // Detect connection and orientation
+  if((sbu1_voltage < HARNESS_CONNECTED_THRESHOLD) || (sbu2_voltage < HARNESS_CONNECTED_THRESHOLD)){
+    if (sbu1_voltage < sbu2_voltage) {
+      // orientation normal
+      ret = HARNESS_STATUS_NORMAL;
+    } else {
+      // orientation flipped
+      ret = HARNESS_STATUS_FLIPPED;
+    }
+  }
+  #endif
+
+  return ret;
+}
+
+void harness_init(void) {
+  // delay such that the connection is fully made before trying orientation detection
+  current_board->set_led(LED_BLUE, true);
+  delay(10000000);
+  current_board->set_led(LED_BLUE, false);
+
+  // try to detect orientation
+  uint8_t ret = harness_detect_orientation();
+  if (ret != HARNESS_STATUS_NC) {
+    puts("detected car harness with orientation "); puth2(ret); puts("\n");
+    car_harness_status = ret;
+
+    // set the SBU lines to be inputs before using the relay. The lines are not 5V tolerant in ADC mode!
+    set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT);
+    set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT);
+
+    // now we have orientation, set pin ignition detection
+    if(car_harness_status == HARNESS_STATUS_NORMAL){
+      set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT);
+    } else {
+      set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT);
+    }      
+
+    // keep busses connected by default
+    set_intercept_relay(false);
+
+    // setup ignition interrupts
+    harness_setup_ignition_interrupts();
+  } else {
+    puts("failed to detect car harness!\n");
+  }
+}

panda/board/drivers/llgpio.h

@@ -7,6 +7,9 @@
 #define PULL_UP 1
 #define PULL_DOWN 2
 
+#define OUTPUT_TYPE_PUSH_PULL 0U
+#define OUTPUT_TYPE_OPEN_DRAIN 1U
+
 void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
   uint32_t tmp = GPIO->MODER;
   tmp &= ~(3U << (pin * 2U));
@@ -23,6 +26,14 @@ void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) {
   set_gpio_mode(GPIO, pin, MODE_OUTPUT);
 }
 
+void set_gpio_output_type(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int output_type){
+  if(output_type == OUTPUT_TYPE_OPEN_DRAIN) {
+    GPIO->OTYPER |= (1U << pin);
+  } else {
+    GPIO->OTYPER &= ~(1U << pin);
+  }
+}
+
 void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
   uint32_t tmp = GPIO->AFR[pin >> 3U];
   tmp &= ~(0xFU << ((pin & 7U) * 4U));

panda/board/gpio.h

@@ -1,383 +1,4 @@
-// this is last place with ifdef PANDA
-
-#ifdef STM32F4
-  #include "stm32f4xx_hal_gpio_ex.h"
-#else
-  #include "stm32f2xx_hal_gpio_ex.h"
-#endif
-
-// ********************* dynamic configuration detection *********************
-
-#define PANDA_REV_AB 0
-#define PANDA_REV_C 1
-
-#define PULL_EFFECTIVE_DELAY 10
-
-void puts(const char *a);
-
-bool has_external_debug_serial = 0;
-bool is_giant_panda = 0;
-bool is_entering_bootmode = 0;
-int revision = PANDA_REV_AB;
-bool is_grey_panda = 0;
-
-bool detect_with_pull(GPIO_TypeDef *GPIO, int pin, int mode) {
-  set_gpio_mode(GPIO, pin, MODE_INPUT);
-  set_gpio_pullup(GPIO, pin, mode);
-  for (volatile int i=0; i<PULL_EFFECTIVE_DELAY; i++);
-  bool ret = get_gpio_input(GPIO, pin);
-  set_gpio_pullup(GPIO, pin, PULL_NONE);
-  return ret;
-}
-
-// must call again from main because BSS is zeroed
-void detect(void) {
-  // detect has_external_debug_serial
-  has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);
-
-#ifdef PANDA
-  // detect is_giant_panda
-  is_giant_panda = detect_with_pull(GPIOB, 1, PULL_DOWN);
-
-  // detect panda REV C.
-  // A13 floats in REV AB. In REV C, A13 is pulled up to 5V with a 10K
-  // resistor and attached to the USB power control chip CTRL
-  // line. Pulling A13 down with an internal 50k resistor in REV C
-  // will produce a voltage divider that results in a high logic
-  // level. Checking if this pin reads high with a pull down should
-  // differentiate REV AB from C.
-  revision = detect_with_pull(GPIOA, 13, PULL_DOWN) ? PANDA_REV_C : PANDA_REV_AB;
-
-  // check if the ESP is trying to put me in boot mode
-  is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
-
-  // check if it's a grey panda by seeing if the SPI lines are floating
-  // TODO: is this reliable?
-  is_grey_panda = !(detect_with_pull(GPIOA, 4, PULL_DOWN) | detect_with_pull(GPIOA, 5, PULL_DOWN) | detect_with_pull(GPIOA, 6, PULL_DOWN) | detect_with_pull(GPIOA, 7, PULL_DOWN));
-#else
-  // need to do this for early detect
-  is_giant_panda = 0;
-  is_grey_panda = 0;
-  revision = PANDA_REV_AB;
-  is_entering_bootmode = 0;
-#endif
-}
-
-// ********************* bringup *********************
-
-void periph_init(void) {
-  // enable GPIOB, UART2, CAN, USB clock
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
-
-  RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
-  RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
-  RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
-  #ifdef PANDA
-    RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
-  #endif
-  RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
-  RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
-  #ifdef CAN3
-    RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
-  #endif
-  RCC->APB1ENR |= RCC_APB1ENR_DACEN;
-  RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;  // main counter
-  RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;  // slow loop and pedal
-  RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;  // gmlan_alt
-  //RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
-  //RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;
-  RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
-  RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
-  //RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
-}
-
-// ********************* setters *********************
-
-void set_can_enable(CAN_TypeDef *CAN_obj, bool enabled) {
-  // enable CAN busses
-  if (CAN_obj == CAN1) {
-    #ifdef PANDA
-      // CAN1_EN
-      set_gpio_output(GPIOC, 1, !enabled);
-    #else
-      #ifdef PEDAL
-        // CAN1_EN (not flipped)
-        set_gpio_output(GPIOB, 3, !enabled);
-      #else
-        // CAN1_EN
-        set_gpio_output(GPIOB, 3, enabled);
-      #endif
-    #endif
-  } else if (CAN_obj == CAN2) {
-    #ifdef PANDA
-      // CAN2_EN
-      set_gpio_output(GPIOC, 13, !enabled);
-    #else
-      // CAN2_EN
-      set_gpio_output(GPIOB, 4, enabled);
-    #endif
-  #ifdef CAN3
-  } else if (CAN_obj == CAN3) {
-    // CAN3_EN
-    set_gpio_output(GPIOA, 0, !enabled);
-  #endif
-  } else {
-    puts("Invalid CAN: enabling failed\n");
-  }
-}
-
-#ifdef PANDA
-  #define LED_RED 9
-  #define LED_GREEN 7
-  #define LED_BLUE 6
-#else
-  #define LED_RED 10
-  #define LED_GREEN 11
-  #define LED_BLUE -1
-#endif
-
-void set_led(int led_num, int on) {
-  if (led_num != -1) {
-  #ifdef PANDA
-    set_gpio_output(GPIOC, led_num, !on);
-  #else
-    set_gpio_output(GPIOB, led_num, !on);
-  #endif
-  }
-}
-
-void set_can_mode(int can, bool use_gmlan) {
-  // connects to CAN2 xcvr or GMLAN xcvr
-  if (use_gmlan) {
-    if (can == 1) {
-      // B5,B6: disable normal mode
-      set_gpio_mode(GPIOB, 5, MODE_INPUT);
-      set_gpio_mode(GPIOB, 6, MODE_INPUT);
-
-      // B12,B13: gmlan mode
-      set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
-      set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
-#ifdef CAN3
-    } else if (can == 2) {
-      // A8,A15: disable normal mode
-      set_gpio_mode(GPIOA, 8, MODE_INPUT);
-      set_gpio_mode(GPIOA, 15, MODE_INPUT);
-
-      // B3,B4: enable gmlan mode
-      set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3);
-      set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3);
-#endif
-    } else {
-      puts("Invalid CAN: mode setting failed\n");
-    }
-  } else {
-    if (can == 1) {
-      // B12,B13: disable gmlan mode
-      set_gpio_mode(GPIOB, 12, MODE_INPUT);
-      set_gpio_mode(GPIOB, 13, MODE_INPUT);
-
-      // B5,B6: normal mode
-      set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
-      set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
-#ifdef CAN3
-    } else if (can == 2) {
-      // B3,B4: disable gmlan mode
-      set_gpio_mode(GPIOB, 3, MODE_INPUT);
-      set_gpio_mode(GPIOB, 4, MODE_INPUT);
-      // A8,A15: normal mode
-      set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
-      set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
-#endif
-    } else {
-      puts("Invalid CAN: mode setting failed\n");
-    }
-  }
-}
-
-#define USB_POWER_NONE 0
-#define USB_POWER_CLIENT 1
-#define USB_POWER_CDP 2
-#define USB_POWER_DCP 3
-
-int usb_power_mode = USB_POWER_NONE;
-
-void set_usb_power_mode(int mode) {
-  bool valid_mode = true;
-  switch (mode) {
-    case USB_POWER_CLIENT:
-      // B2,A13: set client mode
-      set_gpio_output(GPIOB, 2, 0);
-      set_gpio_output(GPIOA, 13, 1);
-      break;
-    case USB_POWER_CDP:
-      // B2,A13: set CDP mode
-      set_gpio_output(GPIOB, 2, 1);
-      set_gpio_output(GPIOA, 13, 1);
-      break;
-    case USB_POWER_DCP:
-      // B2,A13: set DCP mode on the charger (breaks USB!)
-      set_gpio_output(GPIOB, 2, 0);
-      set_gpio_output(GPIOA, 13, 0);
-      break;
-    default:
-      valid_mode = false;
-      puts("Invalid usb power mode\n");
-      break;
-  }
-
-  if (valid_mode) {
-    usb_power_mode = mode;
-  }
-}
-
-#define ESP_DISABLED 0
-#define ESP_ENABLED 1
-#define ESP_BOOTMODE 2
-
-void set_esp_mode(int mode) {
-  switch (mode) {
-    case ESP_DISABLED:
-      // ESP OFF
-      set_gpio_output(GPIOC, 14, 0);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    case ESP_ENABLED:
-      // ESP ON
-      set_gpio_output(GPIOC, 14, 1);
-      set_gpio_output(GPIOC, 5, 1);
-      break;
-    case ESP_BOOTMODE:
-      set_gpio_output(GPIOC, 14, 1);
-      set_gpio_output(GPIOC, 5, 0);
-      break;
-    default:
-      puts("Invalid esp mode\n");
-      break;
-  }
-}
-
-// ********************* big init function *********************
-
-// board specific
-void gpio_init(void) {
-  // pull low to hold ESP in reset??
-  // enable OTG out tied to ground
-  GPIOA->ODR = 0;
-  GPIOB->ODR = 0;
-  GPIOA->PUPDR = 0;
-  //GPIOC->ODR = 0;
-  GPIOB->AFR[0] = 0;
-  GPIOB->AFR[1] = 0;
-
-  // C2,C3: analog mode, voltage and current sense
-  set_gpio_mode(GPIOC, 2, MODE_ANALOG);
-  set_gpio_mode(GPIOC, 3, MODE_ANALOG);
-
-#ifdef PEDAL
-  // comma pedal has inputs on C0 and C1
-  set_gpio_mode(GPIOC, 0, MODE_ANALOG);
-  set_gpio_mode(GPIOC, 1, MODE_ANALOG);
-  // DAC outputs on A4 and A5
-  //   apparently they don't need GPIO setup
-#endif
-
-  // C8: FAN aka TIM3_CH4
-  set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
-
-  // turn off LEDs and set mode
-  set_led(LED_RED, 0);
-  set_led(LED_GREEN, 0);
-  set_led(LED_BLUE, 0);
-
-  // A11,A12: USB
-  set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS);
-  set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
-  GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
-
-#ifdef PANDA
-  // enable started_alt on the panda
-  set_gpio_pullup(GPIOA, 1, PULL_UP);
-
-  // A2,A3: USART 2 for debugging
-  set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2);
-  set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2);
-
-  // A9,A10: USART 1 for talking to the ESP
-  set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
-  set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
-
-  // B12: GMLAN, ignition sense, pull up
-  set_gpio_pullup(GPIOB, 12, PULL_UP);
-
-  // A4,A5,A6,A7: setup SPI
-  set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
-  set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
-  set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1);
-  set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
-#endif
-
-  // B8,B9: CAN 1
-#ifdef STM32F4
-  set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
-  set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
-#else
-  set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
-  set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
-#endif
-  set_can_enable(CAN1, 1);
-
-  // B5,B6: CAN 2
-  set_can_mode(1, 0);
-  set_can_enable(CAN2, 1);
-
-  // A8,A15: CAN 3
-  #ifdef CAN3
-    set_can_mode(2, 0);
-    set_can_enable(CAN3, 1);
-  #endif
-
-  /* GMLAN mode pins:
-  M0(B15)  M1(B14)  mode
-  =======================
-  0        0        sleep
-  1        0        100kbit
-  0        1        high voltage wakeup
-  1        1        33kbit (normal)
-  */
-
-  // put gmlan transceiver in normal mode
-  set_gpio_output(GPIOB, 14, 1);
-  set_gpio_output(GPIOB, 15, 1);
-
-  #ifdef PANDA
-    // K-line enable moved from B4->B7 to make room for GMLAN on CAN3
-    set_gpio_output(GPIOB, 7, 1); // REV C
-
-    // C12,D2: K-Line setup on UART 5
-    set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5);
-    set_gpio_alternate(GPIOD, 2, GPIO_AF8_UART5);
-    set_gpio_pullup(GPIOD, 2, PULL_UP);
-
-    // L-line enable
-    set_gpio_output(GPIOA, 14, 1);
-
-    // C10,C11: L-Line setup on USART 3
-    set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3);
-    set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
-    set_gpio_pullup(GPIOC, 11, PULL_UP);
-  #endif
-
-  set_usb_power_mode(USB_POWER_CLIENT);
-}
-
-// ********************* early bringup *********************
-
+// Early bringup
 #define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
 #define ENTER_SOFTLOADER_MAGIC 0xdeadc0de
 #define BOOT_NORMAL 0xdeadb111
@@ -429,29 +50,25 @@ void early(void) {
   GPIOA->ODR = 0; GPIOB->ODR = 0; GPIOC->ODR = 0;
   GPIOA->PUPDR = 0; GPIOB->PUPDR = 0; GPIOC->PUPDR = 0;
 
-  detect();
+  detect_configuration();
+  detect_board_type();
 
   #ifdef PANDA
     // enable the ESP, disable ESP boot mode
-    // unless we are on a giant panda, then there's no ESP
     // dont disable on grey panda
-    if (is_giant_panda) {
-      set_esp_mode(ESP_DISABLED);
-    } else {
-      set_esp_mode(ESP_ENABLED);
-    }
+    current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
   #endif
 
 
   if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
   #ifdef PANDA
-    set_esp_mode(ESP_DISABLED);
+    current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
   #endif
-    set_led(LED_GREEN, 1);
+    current_board->set_led(LED_GREEN, 1);
     jump_to_bootloader();
   }
 
   if (is_entering_bootmode) {
     enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
   }
-}
+}

panda/board/main.c

@@ -1,34 +1,39 @@
-//#define EON
+//#define EON 
+//#define PANDA
 
+// ********************* Includes *********************
 #include "config.h"
 #include "obj/gitversion.h"
 
-// ********************* includes *********************
-
-
 #include "libc.h"
 #include "provision.h"
 
+#include "main_declarations.h"
+
 #include "drivers/llcan.h"
 #include "drivers/llgpio.h"
-#include "gpio.h"
+#include "drivers/adc.h"
+
+#include "board.h"
 
 #include "drivers/uart.h"
-#include "drivers/adc.h"
 #include "drivers/usb.h"
 #include "drivers/gmlan_alt.h"
 #include "drivers/timer.h"
 #include "drivers/clock.h"
 
+#include "gpio.h"
+
 #ifndef EON
 #include "drivers/spi.h"
 #endif
 
 #include "power_saving.h"
 #include "safety.h"
+
 #include "drivers/can.h"
 
-// ********************* serial debugging *********************
+// ********************* Serial debugging *********************
 
 void debug_ring_callback(uart_ring *ring) {
   char rcv;
@@ -49,30 +54,23 @@ void debug_ring_callback(uart_ring *ring) {
     // enable CDP mode
     if (rcv == 'C') {
       puts("switching USB to CDP mode\n");
-      set_usb_power_mode(USB_POWER_CDP);
+      current_board->set_usb_power_mode(USB_POWER_CDP);
     }
     if (rcv == 'c') {
       puts("switching USB to client mode\n");
-      set_usb_power_mode(USB_POWER_CLIENT);
+      current_board->set_usb_power_mode(USB_POWER_CLIENT);
     }
     if (rcv == 'D') {
       puts("switching USB to DCP mode\n");
-      set_usb_power_mode(USB_POWER_DCP);
+      current_board->set_usb_power_mode(USB_POWER_DCP);
     }
   }
 }
 
 // ***************************** started logic *****************************
-
-bool is_gpio_started(void) {
-  // ignition is on PA1
-  return (GPIOA->IDR & (1U << 1)) == 0;
-}
-
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void EXTI1_IRQHandler(void) {
-  volatile unsigned int pr = EXTI->PR & (1U << 1);
-  if ((pr & (1U << 1)) != 0U) {
+void started_interrupt_handler(uint8_t interrupt_line) {
+  volatile unsigned int pr = EXTI->PR & (1U << interrupt_line);
+  if ((pr & (1U << interrupt_line)) != 0U) {
     #ifdef DEBUG
       puts("got started interrupt\n");
     #endif
@@ -81,10 +79,25 @@ void EXTI1_IRQHandler(void) {
     delay(100000);
 
     // set power savings mode here
-    int power_save_state = is_gpio_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
+    int power_save_state = current_board->check_ignition() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
     set_power_save_state(power_save_state);
-    EXTI->PR = (1U << 1);
   }
+  EXTI->PR = (1U << interrupt_line);
+}
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void EXTI0_IRQHandler(void) {
+  started_interrupt_handler(0);
+}
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void EXTI1_IRQHandler(void) {
+  started_interrupt_handler(1);
+}
+
+// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
+void EXTI3_IRQHandler(void) {
+  started_interrupt_handler(3);
 }
 
 void started_interrupt_init(void) {
@@ -95,18 +108,64 @@ void started_interrupt_init(void) {
   NVIC_EnableIRQ(EXTI1_IRQn);
 }
 
+// ****************************** safety mode ******************************
+
+// this is the only way to leave silent mode
+void set_safety_mode(uint16_t mode, int16_t param) {
+  int err = safety_set_mode(mode, param);
+  if (err == -1) {
+    puts("Error: safety set mode failed\n");
+  } else {
+    if (mode == SAFETY_NOOUTPUT) {
+      can_silent = ALL_CAN_SILENT;
+    } else {
+      can_silent = ALL_CAN_LIVE;
+    }
+
+    switch (mode) {
+        case SAFETY_NOOUTPUT:
+          set_intercept_relay(false);
+          if(hw_type == HW_TYPE_BLACK_PANDA){
+            current_board->set_can_mode(CAN_MODE_NORMAL);
+          }
+          break;
+        case SAFETY_ELM327:
+          set_intercept_relay(false);
+          if(hw_type == HW_TYPE_BLACK_PANDA){
+            current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+          }
+          break;
+        default:
+          set_intercept_relay(true);
+          if(hw_type == HW_TYPE_BLACK_PANDA){
+            current_board->set_can_mode(CAN_MODE_NORMAL);
+          }
+          break;
+      }          
+    if (safety_ignition_hook() != -1) {
+      // if the ignition hook depends on something other than the started GPIO
+      // we have to disable power savings (fix for GM and Tesla)
+      set_power_save_state(POWER_SAVE_STATUS_DISABLED);
+    } else {
+      // power mode is already POWER_SAVE_STATUS_DISABLED and CAN TXs are active
+    }
+    can_init_all();
+  }
+}
+
 // ***************************** USB port *****************************
 
 int get_health_pkt(void *dat) {
   struct __attribute__((packed)) {
     uint32_t voltage_pkt;
     uint32_t current_pkt;
-    uint8_t started_pkt;
-    uint8_t controls_allowed_pkt;
-    uint8_t gas_interceptor_detected_pkt;
     uint32_t can_send_errs_pkt;
     uint32_t can_fwd_errs_pkt;
     uint32_t gmlan_send_errs_pkt;
+    uint8_t started_pkt;
+    uint8_t controls_allowed_pkt;
+    uint8_t gas_interceptor_detected_pkt;
+    uint8_t car_harness_status_pkt;
   } *health = dat;
 
   //Voltage will be measured in mv. 5000 = 5V
@@ -121,11 +180,17 @@ int get_health_pkt(void *dat) {
   // Avoid needing floating point math
   health->voltage_pkt = (voltage * 8862U) / 1000U;
 
-  health->current_pkt = adc_get(ADCCHAN_CURRENT);
+  // No current sense on panda black
+  if(hw_type != HW_TYPE_BLACK_PANDA){
+    health->current_pkt = adc_get(ADCCHAN_CURRENT);
+  } else {
+    health->current_pkt = 0;
+  }
+
   int safety_ignition = safety_ignition_hook();
   if (safety_ignition < 0) {
     //Use the GPIO pin to determine ignition
-    health->started_pkt = is_gpio_started();
+    health->started_pkt = (uint8_t)(current_board->check_ignition());
   } else {
     //Current safety hooks want to determine ignition (ex: GM)
     health->started_pkt = safety_ignition;
@@ -136,7 +201,8 @@ int get_health_pkt(void *dat) {
   health->can_send_errs_pkt = can_send_errs;
   health->can_fwd_errs_pkt = can_fwd_errs;
   health->gmlan_send_errs_pkt = gmlan_send_errs;
-
+  health->car_harness_status_pkt = car_harness_status;
+  
   return sizeof(*health);
 }
 
@@ -183,8 +249,6 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
   }
 }
 
-bool is_enumerated = 0;
-
 void usb_cb_enumeration_complete() {
   puts("USB enumeration complete\n");
   is_enumerated = 1;
@@ -203,9 +267,9 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
       puts(" err: "); puth(can_err_cnt);
       puts("\n");
       break;
-    // **** 0xc1: is grey panda
+    // **** 0xc1: get hardware type
     case 0xc1:
-      resp[0] = is_grey_panda;
+      resp[0] = hw_type;
       resp_len = 1;
       break;
     // **** 0xd0: fetch serial number
@@ -258,73 +322,57 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
     // **** 0xd9: set ESP power
     case 0xd9:
       if (setup->b.wValue.w == 1U) {
-        set_esp_mode(ESP_ENABLED);
+        current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
       } else if (setup->b.wValue.w == 2U) {
-        set_esp_mode(ESP_BOOTMODE);
+        current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
       } else {
-        set_esp_mode(ESP_DISABLED);
+        current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
       }
       break;
     // **** 0xda: reset ESP, with optional boot mode
     case 0xda:
-      set_esp_mode(ESP_DISABLED);
+      current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
       delay(1000000);
       if (setup->b.wValue.w == 1U) {
-        set_esp_mode(ESP_BOOTMODE);
+        current_board->set_esp_gps_mode(ESP_GPS_BOOTMODE);
       } else {
-        set_esp_mode(ESP_ENABLED);
+        current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
       }
       delay(1000000);
-      set_esp_mode(ESP_ENABLED);
+      current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
       break;
-    // **** 0xdb: set GMLAN multiplexing mode
+    // **** 0xdb: set GMLAN (white/grey) or OBD CAN (black) multiplexing mode
     case 0xdb:
-      if (setup->b.wValue.w == 1U) {
-        // GMLAN ON
-        if (setup->b.wIndex.w == 1U) {
-          can_set_gmlan(1);
-        } else if (setup->b.wIndex.w == 2U) {
-          can_set_gmlan(2);
+      if(hw_type == HW_TYPE_BLACK_PANDA){
+        if (setup->b.wValue.w == 1U) {
+          // Enable OBD CAN
+          current_board->set_can_mode(CAN_MODE_OBD_CAN2);
         } else {
-          puts("Invalid bus num for GMLAN CAN set\n");
-        }
+          // Disable OBD CAN
+          current_board->set_can_mode(CAN_MODE_NORMAL);
+        }        
       } else {
-        can_set_gmlan(-1);
+        if (setup->b.wValue.w == 1U) {
+          // GMLAN ON
+          if (setup->b.wIndex.w == 1U) {
+            can_set_gmlan(1);
+          } else if (setup->b.wIndex.w == 2U) {
+            can_set_gmlan(2);
+          } else {
+            puts("Invalid bus num for GMLAN CAN set\n");
+          }
+        } else {
+          can_set_gmlan(-1);
+        }
       }
       break;
+      
     // **** 0xdc: set safety mode
     case 0xdc:
-      // this is the only way to leave silent mode
-      // and it's blocked over WiFi
-      // Allow ELM security mode to be set over wifi.
+      // Blocked over WiFi.
+      // Allow NOOUTPUT and ELM security mode to be set over wifi.
       if (hardwired || (setup->b.wValue.w == SAFETY_NOOUTPUT) || (setup->b.wValue.w == SAFETY_ELM327)) {
-        int err = safety_set_mode(setup->b.wValue.w, (int16_t)setup->b.wIndex.w);
-        if (err == -1) {
-          puts("Error: safety set mode failed\n");
-        } else {
-          #ifndef EON
-            // always LIVE on EON
-            switch (setup->b.wValue.w) {
-              case SAFETY_NOOUTPUT:
-                can_silent = ALL_CAN_SILENT;
-                break;
-              case SAFETY_ELM327:
-                can_silent = ALL_CAN_BUT_MAIN_SILENT;
-                break;
-              default:
-                can_silent = ALL_CAN_LIVE;
-                break;
-            }
-          #endif
-          if (safety_ignition_hook() != -1) {
-            // if the ignition hook depends on something other than the started GPIO
-            // we have to disable power savings (fix for GM and Tesla)
-            set_power_save_state(POWER_SAVE_STATUS_DISABLED);
-          } else {
-            // power mode is already POWER_SAVE_STATUS_DISABLED and CAN TXs are active
-          }
-          can_init_all();
-        }
+        set_safety_mode(setup->b.wValue.w, (uint16_t) setup->b.wIndex.w);
       }
       break;
     // **** 0xdd: enable can forwarding
@@ -418,13 +466,13 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
     case 0xe6:
       if (setup->b.wValue.w == 1U) {
         puts("user setting CDP mode\n");
-        set_usb_power_mode(USB_POWER_CDP);
+        current_board->set_usb_power_mode(USB_POWER_CDP);
       } else if (setup->b.wValue.w == 2U) {
         puts("user setting DCP mode\n");
-        set_usb_power_mode(USB_POWER_DCP);
+        current_board->set_usb_power_mode(USB_POWER_DCP);
       } else {
         puts("user setting CLIENT mode\n");
-        set_usb_power_mode(USB_POWER_CLIENT);
+        current_board->set_usb_power_mode(USB_POWER_CLIENT);
       }
       break;
     // **** 0xf0: do k-line wValue pulse on uart2 for Acura
@@ -482,6 +530,12 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
         }
         break;
       }
+    // **** 0xf3: Heartbeat. Resets heartbeat counter.
+    case 0xf3:
+      {
+        heartbeat_counter = 0U;
+        break;
+      }
     default:
       puts("NO HANDLER ");
       puth(setup->b.bRequest);
@@ -536,95 +590,52 @@ void __attribute__ ((noinline)) enable_fpu(void) {
 }
 
 uint64_t tcnt = 0;
-uint64_t marker = 0;
+
+// go into NOOUTPUT when the EON does not send a heartbeat for this amount of seconds.
+#define EON_HEARTBEAT_THRESHOLD_IGNITION_ON 5U
+#define EON_HEARTBEAT_THRESHOLD_IGNITION_OFF 2U
 
 // called once per second
 // cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
 void TIM3_IRQHandler(void) {
-  #define CURRENT_THRESHOLD 0xF00U
-  #define CLICKS 5U // 5 seconds to switch modes
-
   if (TIM3->SR != 0) {
     can_live = pending_can_live;
 
+    current_board->usb_power_mode_tick(tcnt);
+
     //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n");
 
-    uint32_t current = adc_get(ADCCHAN_CURRENT);
-
-    switch (usb_power_mode) {
-      case USB_POWER_CLIENT:
-        if ((tcnt - marker) >= CLICKS) {
-          if (!is_enumerated) {
-            puts("USBP: didn't enumerate, switching to CDP mode\n");
-            // switch to CDP
-            set_usb_power_mode(USB_POWER_CDP);
-            marker = tcnt;
-          }
-        }
-        // keep resetting the timer if it's enumerated
-        if (is_enumerated) {
-          marker = tcnt;
-        }
-        break;
-      case USB_POWER_CDP:
-        // On the EON, if we get into CDP mode we stay here. No need to go to DCP.
-        #ifndef EON
-          // been CLICKS clicks since we switched to CDP
-          if ((tcnt-marker) >= CLICKS) {
-            // measure current draw, if positive and no enumeration, switch to DCP
-            if (!is_enumerated && (current < CURRENT_THRESHOLD)) {
-              puts("USBP: no enumeration with current draw, switching to DCP mode\n");
-              set_usb_power_mode(USB_POWER_DCP);
-              marker = tcnt;
-            }
-          }
-          // keep resetting the timer if there's no current draw in CDP
-          if (current >= CURRENT_THRESHOLD) {
-            marker = tcnt;
-          }
-        #endif
-        break;
-      case USB_POWER_DCP:
-        // been at least CLICKS clicks since we switched to DCP
-        if ((tcnt-marker) >= CLICKS) {
-          // if no current draw, switch back to CDP
-          if (current >= CURRENT_THRESHOLD) {
-            puts("USBP: no current draw, switching back to CDP mode\n");
-            set_usb_power_mode(USB_POWER_CDP);
-            marker = tcnt;
-          }
-        }
-        // keep resetting the timer if there's current draw in DCP
-        if (current < CURRENT_THRESHOLD) {
-          marker = tcnt;
-        }
-        break;
-      default:
-        puts("USB power mode invalid\n");  // set_usb_power_mode prevents assigning invalid values
-        break;
-    }
-
-    // ~0x9a = 500 ma
-    /*puth(current);
-    puts("\n");*/
-
     // reset this every 16th pass
     if ((tcnt & 0xFU) == 0U) {
       pending_can_live = 0;
     }
     #ifdef DEBUG
-      puts("** blink ");
-      puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts("  ");
-      puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts("  ");
-      puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
+      //TODO: re-enable
+      //puts("** blink ");
+      //puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts("  ");
+      //puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts("  ");
+      //puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
     #endif
 
     // set green LED to be controls allowed
-    set_led(LED_GREEN, controls_allowed);
+    current_board->set_led(LED_GREEN, controls_allowed);
 
     // turn off the blue LED, turned on by CAN
     // unless we are in power saving mode
-    set_led(LED_BLUE, (tcnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
+    current_board->set_led(LED_BLUE, (tcnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
+
+    // increase heartbeat counter and cap it at the uint32 limit
+    if (heartbeat_counter < __UINT32_MAX__) {
+      heartbeat_counter += 1U;
+    }
+
+    // check heartbeat counter if we are running EON code. If the heartbeat has been gone for a while, go to NOOUTPUT safety mode.
+    #ifdef EON
+    if (heartbeat_counter >= (current_board->check_ignition() ? EON_HEARTBEAT_THRESHOLD_IGNITION_ON : EON_HEARTBEAT_THRESHOLD_IGNITION_OFF)) {
+      puts("EON hasn't sent a heartbeat for 0x"); puth(heartbeat_counter); puts(" seconds. Safety is set to NOOUTPUT mode.\n");
+      set_safety_mode(SAFETY_NOOUTPUT, 0U);
+    }
+    #endif
 
     // on to the next one
     tcnt += 1U;
@@ -638,26 +649,27 @@ int main(void) {
 
   // init early devices
   clock_init();
-  periph_init();
-  detect();
-
+  peripherals_init();
+  detect_configuration();
+  detect_board_type();
+  adc_init();
+  
   // print hello
   puts("\n\n\n************************ MAIN START ************************\n");
 
-  // detect the revision and init the GPIOs
-  puts("config:\n");
-  puts((revision == PANDA_REV_C) ? "  panda rev c\n" : "  panda rev a or b\n");
-  puts(has_external_debug_serial ? "  real serial\n" : "  USB serial\n");
-  puts(is_giant_panda ? "  GIANTpanda detected\n" : "  not GIANTpanda\n");
-  puts(is_grey_panda ? "  gray panda detected!\n" : "  white panda\n");
-  puts(is_entering_bootmode ? "  ESP wants bootmode\n" : "  no bootmode\n");
-
-  // non rev c panda are no longer supported
-  while (revision != PANDA_REV_C) {
-    // hang
+  // check for non-supported board types
+  if(hw_type == HW_TYPE_UNKNOWN){
+    puts("Unsupported board type\n");
+    while (1) { /* hang */ }
   }
 
-  gpio_init();
+  puts("Config:\n");
+  puts("  Board type: "); puts(current_board->board_type); puts("\n");
+  puts(has_external_debug_serial ? "  Real serial\n" : "  USB serial\n");
+  puts(is_entering_bootmode ? "  ESP wants bootmode\n" : "  No bootmode\n");
+
+  // init board
+  current_board->init();
 
   // panda has an FPU, let's use it!
   enable_fpu();
@@ -669,18 +681,21 @@ int main(void) {
     uart_init(USART2, 115200);
   }
 
-  if (is_grey_panda) {
+  if (board_has_gps()) {
     uart_init(USART1, 9600);
   } else {
     // enable ESP uart
     uart_init(USART1, 115200);
   }
 
-  // enable LIN
-  uart_init(UART5, 10400);
-  UART5->CR2 |= USART_CR2_LINEN;
-  uart_init(USART3, 10400);
-  USART3->CR2 |= USART_CR2_LINEN;
+  // there is no LIN on panda black
+  if(hw_type != HW_TYPE_BLACK_PANDA){
+    // enable LIN
+    uart_init(UART5, 10400);
+    UART5->CR2 |= USART_CR2_LINEN;
+    uart_init(USART3, 10400);
+    USART3->CR2 |= USART_CR2_LINEN;
+  }
 
   // init microsecond system timer
   // increments 1000000 times per second
@@ -690,9 +705,6 @@ int main(void) {
   TIM2->EGR = TIM_EGR_UG;
   // use TIM2->CNT to read
 
-  // enable USB
-  usb_init();
-
   // default to silent mode to prevent issues with Ford
   // hardcode a specific safety mode if you want to force the panda to be in a specific mode
   int err = safety_set_mode(SAFETY_NOOUTPUT, 0);
@@ -702,31 +714,27 @@ int main(void) {
       // if SAFETY_NOOUTPUT isn't succesfully set, we can't continue
     }
   }
-#ifdef EON
-  // if we're on an EON, it's fine for CAN to be live for fingerprinting
-  can_silent = ALL_CAN_LIVE;
-#else
   can_silent = ALL_CAN_SILENT;
-#endif
   can_init_all();
 
-  adc_init();
-
 #ifndef EON
   spi_init();
 #endif
 
 #ifdef EON
   // have to save power
-  if (!is_grey_panda) {
-    set_esp_mode(ESP_DISABLED);
+  if (hw_type == HW_TYPE_WHITE_PANDA) {
+    current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
   }
   // only enter power save after the first cycle
-  /*if (is_gpio_started()) {
+  /*if (current_board->check_ignition()) {
     set_power_save_state(POWER_SAVE_STATUS_ENABLED);
   }*/
-  // interrupt on started line
-  started_interrupt_init();
+
+  if (hw_type != HW_TYPE_BLACK_PANDA) {
+    // interrupt on started line
+    started_interrupt_init();
+  }
 #endif
 
   // 48mhz / 65536 ~= 732 / 732 = 1
@@ -736,6 +744,8 @@ int main(void) {
 #ifdef DEBUG
   puts("DEBUG ENABLED\n");
 #endif
+  // enable USB (right before interrupts or enum can fail!)
+  usb_init();
 
   puts("**** INTERRUPTS ON ****\n");
   enable_interrupts();
@@ -751,9 +761,9 @@ int main(void) {
       for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) {
         for (int fade = 0; fade < 1024; fade += 8) {
           for (int i = 0; i < (128/div_mode); i++) {
-            set_led(LED_RED, 1);
+            current_board->set_led(LED_RED, 1);
             if (fade < 512) { delay(fade); } else { delay(1024-fade); }
-            set_led(LED_RED, 0);
+            current_board->set_led(LED_RED, 0);
             if (fade < 512) { delay(512-fade); } else { delay(fade-512); }
           }
         }
@@ -765,4 +775,3 @@ int main(void) {
 
   return 0;
 }
-

panda/board/main_declarations.h

@@ -0,0 +1,14 @@
+// ******************** Prototypes ********************
+void puts(const char *a);
+void puth(unsigned int i);
+void puth2(unsigned int i);
+typedef struct board board;
+typedef struct harness_configuration harness_configuration;
+void can_flip_buses(uint8_t bus1, uint8_t bus2);
+void can_set_obd(uint8_t harness_orientation, bool obd);
+
+// ********************* Globals **********************
+uint8_t hw_type = 0;
+const board *current_board;
+bool is_enumerated = 0;
+uint32_t heartbeat_counter = 0;

panda/board/pedal/main.c

@@ -1,14 +1,20 @@
+// ********************* Includes *********************
 #include "../config.h"
+#include "libc.h"
+
+#include "main_declarations.h"
 
 #include "drivers/llcan.h"
 #include "drivers/llgpio.h"
-#include "drivers/clock.h"
 #include "drivers/adc.h"
+
+#include "board.h"
+
+#include "drivers/clock.h"
 #include "drivers/dac.h"
 #include "drivers/timer.h"
 
 #include "gpio.h"
-#include "libc.h"
 
 #define CAN CAN1
 
@@ -25,6 +31,9 @@
   void puth(unsigned int i) {
     UNUSED(i);
   }
+  void puth2(unsigned int i) {
+    UNUSED(i);
+  }
 #endif
 
 #define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
@@ -180,7 +189,7 @@ void CAN1_RX0_IRQHandler(void) {
         if (((current_index + 1U) & COUNTER_CYCLE) == index) {
           #ifdef DEBUG
             puts("setting gas ");
-            puth(value);
+            puth(value_0);
             puts("\n");
           #endif
           if (enable) {
@@ -257,7 +266,7 @@ void TIM3_IRQHandler(void) {
   }
 
   // blink the LED
-  set_led(LED_GREEN, led_value);
+  current_board->set_led(LED_GREEN, led_value);
   led_value = !led_value;
 
   TIM3->SR = 0;
@@ -294,8 +303,9 @@ int main(void) {
 
   // init devices
   clock_init();
-  periph_init();
-  gpio_init();
+  peripherals_init();
+  detect_configuration();
+  detect_board_type();
 
 #ifdef PEDAL_USB
   // enable USB

panda/board/pedal/main_declarations.h

@@ -0,0 +1,11 @@
+// ******************** Prototypes ********************
+void puts(const char *a);
+void puth(unsigned int i);
+void puth2(unsigned int i);
+typedef struct board board;
+typedef struct harness_configuration harness_configuration;
+
+// ********************* Globals **********************
+uint8_t hw_type = 0;
+const board *current_board;
+bool is_enumerated = 0;

panda/board/power_saving.h

@@ -10,14 +10,14 @@ void set_power_save_state(int state) {
     bool enable = false;
     if (state == POWER_SAVE_STATUS_ENABLED) {
       puts("enable power savings\n");
-      if (is_grey_panda) {
+      if (board_has_gps()) {
         char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
         uart_ring *ur = get_ring_by_number(1);
         for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1U; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
       }
     } else {
       puts("disable power savings\n");
-      if (is_grey_panda) {
+      if (board_has_gps()) {
         char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
         uart_ring *ur = get_ring_by_number(1);
         for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1U; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
@@ -25,18 +25,18 @@ void set_power_save_state(int state) {
       enable = true;
     }
 
-    // turn on can
-    set_can_enable(CAN1, enable);
-    set_can_enable(CAN2, enable);
-    set_can_enable(CAN3, enable);
+    // Switch CAN transcievers
+    current_board->enable_can_transcievers(enable);
 
-    // turn on GMLAN
-    set_gpio_output(GPIOB, 14, enable);
-    set_gpio_output(GPIOB, 15, enable);
+    if(hw_type != HW_TYPE_BLACK_PANDA){
+      // turn on GMLAN
+      set_gpio_output(GPIOB, 14, enable);
+      set_gpio_output(GPIOB, 15, enable);
 
-    // turn on LIN
-    set_gpio_output(GPIOB, 7, enable);
-    set_gpio_output(GPIOA, 14, enable);
+      // turn on LIN    
+      set_gpio_output(GPIOB, 7, enable);
+      set_gpio_output(GPIOA, 14, enable);
+    }
 
     power_save_status = state;
   }

panda/board/safety/safety_elm327.h

@@ -1,15 +1,9 @@
 static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   int tx = 1;
-  int bus = GET_BUS(to_send);
   int addr = GET_ADDR(to_send);
   int len = GET_LEN(to_send);
 
-  //All ELM traffic must appear on CAN0
-  if (bus != 0) {
-    tx = 0;
-  }
-
   //All ISO 15765-4 messages must be 8 bytes long
   if (len != 8) {
     tx = 0;

panda/board/safety/safety_honda.h

@@ -136,8 +136,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
   // ensuring that only the cancel button press is sent (VAL 2) when controls are off.
   // This avoids unintended engagements while still allowing resume spam
+  int bus_pt = ((hw_type == HW_TYPE_BLACK_PANDA) && honda_bosch_hardware)? 1 : 0;
   if ((addr == 0x296) && honda_bosch_hardware &&
-      !current_controls_allowed && (bus == 0)) {
+      !current_controls_allowed && (bus == bus_pt)) {
     if (((GET_BYTE(to_send, 0) >> 5) & 0x7) != 2) {
       tx = 0;
     }
@@ -186,15 +187,17 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
 static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   int bus_fwd = -1;
+  int bus_rdr_cam = (hw_type == HW_TYPE_BLACK_PANDA) ? 2 : 1;  // radar bus, camera side
+  int bus_rdr_car = (hw_type == HW_TYPE_BLACK_PANDA) ? 0 : 2;  // radar bus, car side
 
-  if (bus_num == 2) {
-    bus_fwd = 1;
+  if (bus_num == bus_rdr_car) {
+    bus_fwd = bus_rdr_cam;
   }
-  if (bus_num == 1)  {
+  if (bus_num == bus_rdr_cam)  {
     int addr = GET_ADDR(to_fwd);
     int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
     if (!is_lkas_msg) {
-      bus_fwd = 2;
+      bus_fwd = bus_rdr_car;
     }
   }
   return bus_fwd;

panda/board/spi_flasher.h

@@ -31,7 +31,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
         FLASH->KEYR = 0xCDEF89AB;
         resp[1] = 0xff;
       }
-      set_led(LED_GREEN, 1);
+      current_board->set_led(LED_GREEN, 1);
       unlocked = 1;
       prog_ptr = (uint32_t *)0x8004000;
       break;
@@ -112,7 +112,7 @@ void usb_cb_enumeration_complete(void) {
 
 void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
   UNUSED(hardwired);
-  set_led(LED_RED, 0);
+  current_board->set_led(LED_RED, 0);
   for (int i = 0; i < len/4; i++) {
     // program byte 1
     FLASH->CR = FLASH_CR_PSIZE_1 | FLASH_CR_PG;
@@ -123,7 +123,7 @@ void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
     //*(uint64_t*)(&spi_tx_buf[0x30+(i*4)]) = *prog_ptr;
     prog_ptr++;
   }
-  set_led(LED_RED, 1);
+  current_board->set_led(LED_RED, 1);
 }
 
 
@@ -276,7 +276,7 @@ void soft_flasher_start(void) {
   // B8,B9: CAN 1
   set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
   set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
-  set_can_enable(CAN1, 1);
+  current_board->enable_can_transciever(1, true);
 
   // init can
   llcan_set_speed(CAN1, 5000, false, false);
@@ -305,7 +305,7 @@ void soft_flasher_start(void) {
   usb_init();
 
   // green LED on for flashing
-  set_led(LED_GREEN, 1);
+  current_board->set_led(LED_GREEN, 1);
 
   __enable_irq();
 
@@ -316,13 +316,13 @@ void soft_flasher_start(void) {
       // if you are connected through a hub to the phone
       // you need power to be able to see the device
       puts("USBP: didn't enumerate, switching to CDP mode\n");
-      set_usb_power_mode(USB_POWER_CDP);
-      set_led(LED_BLUE, 1);
+      current_board->set_usb_power_mode(USB_POWER_CDP);
+      current_board->set_led(LED_BLUE, 1);
     }
     // blink the green LED fast
-    set_led(LED_GREEN, 0);
+    current_board->set_led(LED_GREEN, 0);
     delay(500000);
-    set_led(LED_GREEN, 1);
+    current_board->set_led(LED_GREEN, 1);
     delay(500000);
   }
 }

panda/python/__init__.py

@@ -16,7 +16,7 @@ from update import ensure_st_up_to_date
 from serial import PandaSerial
 from isotp import isotp_send, isotp_recv
 
-__version__ = '0.0.8'
+__version__ = '0.0.9'
 
 BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
 
@@ -232,10 +232,10 @@ class Panda(object):
     print("flash: unlocking")
     handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
 
-    # erase sectors 1 and 2
+    # erase sectors 1 through 3
     print("flash: erasing")
-    handle.controlWrite(Panda.REQUEST_IN, 0xb2, 1, 0, b'')
-    handle.controlWrite(Panda.REQUEST_IN, 0xb2, 2, 0, b'')
+    for i in range(1, 4):
+      handle.controlWrite(Panda.REQUEST_IN, 0xb2, i, 0, b'')
 
     # flash over EP2
     STEP = 0x10
@@ -334,13 +334,19 @@ class Panda(object):
   # ******************* health *******************
 
   def health(self):
-    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 13)
-    a = struct.unpack("IIBBBBB", dat)
-    return {"voltage": a[0], "current": a[1],
-            "started": a[2], "controls_allowed": a[3],
-            "gas_interceptor_detected": a[4],
-            "started_signal_detected": a[5],
-            "started_alt": a[6]}
+    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 24)
+    a = struct.unpack("IIIIIBBBB", dat)
+    return {
+      "voltage": a[0],
+      "current": a[1],
+      "can_send_errs": a[2],
+      "can_fwd_errs": a[3],
+      "gmlan_send_errs": a[4],
+      "started": a[5],
+      "controls_allowed": a[6],
+      "gas_interceptor_detected": a[7],
+      "car_harness_status": a[8]
+    }
 
   # ******************* control *******************
 
@@ -354,9 +360,14 @@ class Panda(object):
   def get_version(self):
     return self._handle.controlRead(Panda.REQUEST_IN, 0xd6, 0, 0, 0x40)
 
+  def get_type(self):
+    return self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
+
   def is_grey(self):
-    ret = self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
-    return ret == "\x01"
+    return self.get_type() == "\x02"
+
+  def is_black(self):
+    return self.get_type() == "\x03"
 
   def get_serial(self):
     dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
@@ -387,11 +398,16 @@ class Panda(object):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xdd, from_bus, to_bus, b'')
 
   def set_gmlan(self, bus=2):
+    # TODO: check panda type
     if bus is None:
       self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 0, 0, b'')
     elif bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
       self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 1, bus, b'')
 
+  def set_obd(self, obd):
+    # TODO: check panda type
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, int(obd), 0, b'')
+
   def set_can_loopback(self, enable):
     # set can loopback mode for all buses
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xe5, int(enable), 0, b'')
@@ -559,3 +575,6 @@ class Panda(object):
     msg = self.kline_ll_recv(2, bus=bus)
     msg += self.kline_ll_recv(ord(msg[1])-2, bus=bus)
     return msg
+
+  def send_heartbeat(self):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xf3, 0, 0, b'')

panda/tests/automated/2_usb_to_can.py

@@ -26,6 +26,9 @@ def test_can_loopback(serial=None):
     busses = [0,1,2]
 
   for bus in busses:
+    # send heartbeat
+    p.send_heartbeat()
+
     # set bus 0 speed to 250
     p.set_can_speed_kbps(bus, 250)
 
@@ -52,6 +55,9 @@ def test_safety_nooutput(serial=None):
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_NOOUTPUT)
 
+  # send heartbeat
+  p.send_heartbeat()
+
   # enable CAN loopback mode
   p.set_can_loopback(True)
 
@@ -76,11 +82,17 @@ def test_reliability(serial=None):
   p.set_can_loopback(True)
   p.set_can_speed_kbps(0, 1000)
 
+  # send heartbeat
+  p.send_heartbeat()
+
   addrs = range(100, 100+MSG_COUNT)
   ts = [(j, 0, "\xaa"*8, 0) for j in addrs]
 
   # 100 loops
   for i in range(LOOP_COUNT):
+    # send heartbeat
+    p.send_heartbeat()
+
     st = time.time()
 
     p.can_send_many(ts)
@@ -111,6 +123,9 @@ def test_throughput(serial=None):
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
+  # send heartbeat
+  p.send_heartbeat()
+
   # enable CAN loopback mode
   p.set_can_loopback(True)
 
@@ -119,6 +134,9 @@ def test_throughput(serial=None):
     p.set_can_speed_kbps(0, speed)
     time.sleep(0.05)
 
+    # send heartbeat
+    p.send_heartbeat()
+
     comp_kbps = time_many_sends(p, 0)
 
     # bit count from https://en.wikipedia.org/wiki/CAN_bus
@@ -139,6 +157,9 @@ def test_gmlan(serial=None):
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
+  # send heartbeat
+  p.send_heartbeat()
+
   # enable CAN loopback mode
   p.set_can_loopback(True)
 
@@ -148,6 +169,9 @@ def test_gmlan(serial=None):
 
   # set gmlan on CAN2
   for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
+    # send heartbeat
+    p.send_heartbeat()
+
     p.set_gmlan(bus)
     comp_kbps_gmlan = time_many_sends(p, 3)
     assert_greater(comp_kbps_gmlan, 0.8 * SPEED_GMLAN)
@@ -171,11 +195,17 @@ def test_gmlan_bad_toggle(serial=None):
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
+  # send heartbeat
+  p.send_heartbeat()
+
   # enable CAN loopback mode
   p.set_can_loopback(True)
 
   # GMLAN_CAN2
   for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
+    # send heartbeat
+    p.send_heartbeat()
+
     p.set_gmlan(bus)
     comp_kbps_gmlan = time_many_sends(p, 3)
     assert_greater(comp_kbps_gmlan, 0.6 * SPEED_GMLAN)
@@ -183,6 +213,9 @@ def test_gmlan_bad_toggle(serial=None):
 
   # normal
   for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
+    # send heartbeat
+    p.send_heartbeat()
+
     p.set_gmlan(None)
     comp_kbps_normal = time_many_sends(p, bus)
     assert_greater(comp_kbps_normal, 0.6 * SPEED_NORMAL)

panda/tests/automated/4_wifi_functionality.py

@@ -21,12 +21,18 @@ def test_throughput(serial=None):
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
+  # send heartbeat
+  p.send_heartbeat()
+
   # enable CAN loopback mode
   p.set_can_loopback(True)
 
   p = Panda("WIFI")
 
   for speed in [100,250,500,750,1000]:
+    # send heartbeat
+    p.send_heartbeat()
+
     # set bus 0 speed to speed
     p.set_can_speed_kbps(0, speed)
     time.sleep(0.1)
@@ -46,11 +52,18 @@ def test_recv_only(serial=None):
   connect_wifi(serial)
   p = Panda(serial)
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # send heartbeat
+  p.send_heartbeat()
+
   p.set_can_loopback(True)
   pwifi = Panda("WIFI")
 
   # TODO: msg_count=1000 drops packets, is this fixable?
   for msg_count in [10,100,200]:
+    # send heartbeat
+    p.send_heartbeat()
+
     speed = 500
     p.set_can_speed_kbps(0, speed)
     comp_kbps = time_many_sends(p, 0, pwifi, msg_count)

panda/tests/automated/6_two_panda.py

@@ -13,6 +13,9 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
   p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
   p_send.set_can_loopback(False)
 
+  # send heartbeat
+  p_send.send_heartbeat()
+
   p_recv.set_can_loopback(False)
 
   assert not p_send.legacy
@@ -27,6 +30,9 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
 
   for bus in busses:
     for speed in [100, 250, 500, 750, 1000]:
+      # send heartbeat
+      p_send.send_heartbeat()
+
       p_send.set_can_speed_kbps(bus, speed)
       p_recv.set_can_speed_kbps(bus, speed)
       time.sleep(0.05)
@@ -45,6 +51,10 @@ def test_latency(serial_sender=None, serial_reciever=None):
   p_send = Panda(serial_sender)
   p_recv = Panda(serial_reciever)
 
+  # send heartbeat
+  p_send.send_heartbeat()
+  p_recv.send_heartbeat()
+
   p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
   p_send.set_can_loopback(False)
 
@@ -62,10 +72,18 @@ def test_latency(serial_sender=None, serial_reciever=None):
   p_recv.can_recv()
   p_send.can_recv()
 
+  # send heartbeat
+  p_send.send_heartbeat()  
+  p_recv.send_heartbeat()
+
   busses = [0,1,2]
 
   for bus in busses:
     for speed in [100, 250, 500, 750, 1000]:
+      # send heartbeat
+      p_send.send_heartbeat() 
+      p_recv.send_heartbeat()
+
       p_send.set_can_speed_kbps(bus, speed)
       p_recv.set_can_speed_kbps(bus, speed)
       time.sleep(0.1)

panda/tests/black_loopback_test.py

@@ -0,0 +1,139 @@
+#!/usr/bin/env python
+
+# Loopback test between black panda (+ harness and power) and white/grey panda
+# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test.
+# To be sure, the test should be run with both harness orientations
+
+from __future__ import print_function
+import os
+import sys
+import time
+import random
+import argparse
+
+from hexdump import hexdump
+from itertools import permutations
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+
+def get_test_string():
+  return b"test"+os.urandom(10)
+
+def run_test(sleep_duration):
+  pandas = Panda.list()
+  print(pandas)
+
+  # make sure two pandas are connected
+  if len(pandas) != 2:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # connect
+  pandas[0] = Panda(pandas[0])
+  pandas[1] = Panda(pandas[1])
+
+  # find out which one is black
+  type0 = pandas[0].get_type()
+  type1 = pandas[1].get_type()
+
+  black_panda = None
+  other_panda = None
+  
+  if type0 == "\x03" and type1 != "\x03":
+    black_panda = pandas[0]
+    other_panda = pandas[1]
+  elif type0 != "\x03" and type1 == "\x03":
+    black_panda = pandas[1]
+    other_panda = pandas[0]
+  else:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # disable safety modes
+  black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # test health packet
+  print("black panda health", black_panda.health())
+  print("other panda health", other_panda.health())
+
+  # test black -> other
+  test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration)
+  test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration)
+	
+
+def test_buses(black_panda, other_panda, direction, test_array, sleep_duration):
+  if direction:
+    print("***************** TESTING (BLACK --> OTHER) *****************")
+  else:
+    print("***************** TESTING (OTHER --> BLACK) *****************")
+
+  for send_bus, obd, recv_buses in test_array:
+    black_panda.send_heartbeat()
+    other_panda.send_heartbeat()
+    print("\ntest can: ", send_bus, " OBD: ", obd)
+    
+    # set OBD on black panda
+    black_panda.set_gmlan(True if obd else None)
+
+    # clear and flush
+    if direction:
+      black_panda.can_clear(send_bus)
+    else:
+      other_panda.can_clear(send_bus)
+
+    for recv_bus in recv_buses:
+      if direction:
+        other_panda.can_clear(recv_bus)
+      else:
+	black_panda.can_clear(recv_bus)
+    
+    black_panda.can_recv()
+    other_panda.can_recv()
+
+    # send the characters
+    at = random.randint(1, 2000)
+    st = get_test_string()[0:8]
+    if direction:
+      black_panda.can_send(at, st, send_bus)
+    else:
+      other_panda.can_send(at, st, send_bus)
+    time.sleep(0.1)
+
+    # check for receive
+    if direction:
+      cans_echo = black_panda.can_recv()
+      cans_loop = other_panda.can_recv()
+    else:
+      cans_echo = other_panda.can_recv()
+      cans_loop = black_panda.can_recv()
+
+    loop_buses = []
+    for loop in cans_loop:
+      print("  Loop on bus", str(loop[3]))
+      loop_buses.append(loop[3])
+    if len(cans_loop) == 0:
+      print("  No loop")
+    
+    # test loop buses
+    recv_buses.sort()
+    loop_buses.sort()
+    assert recv_buses == loop_buses
+    print("  TEST PASSED")
+
+    time.sleep(sleep_duration)
+  print("\n")
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("-n", type=int, help="Number of test iterations to run")
+  parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
+  args = parser.parse_args()
+
+  if args.n is None:
+    while True:
+      run_test(sleep_duration=args.sleep)
+  else:
+    for i in range(args.n):
+      run_test(sleep_duration=args.sleep)

panda/tests/safety/libpandasafety_py.py

@@ -37,6 +37,7 @@ bool get_long_controls_allowed(void);
 void set_gas_interceptor_detected(bool c);
 bool get_gas_interceptor_detetcted(void);
 int get_gas_interceptor_prev(void);
+int get_hw_type(void);
 void set_timer(uint32_t t);
 void reset_angle_control(void);
 
@@ -60,6 +61,7 @@ int get_honda_brake_prev(void);
 int get_honda_gas_prev(void);
 void set_honda_alt_brake_msg(bool);
 void set_honda_bosch_hardware(bool);
+int get_honda_bosch_hardware(void);
 
 void init_tests_cadillac(void);
 void set_cadillac_desired_torque_last(int t);

panda/tests/safety/test.c

@@ -1,5 +1,6 @@
 #include <stdint.h>
 #include <stdbool.h>
+#include <stdlib.h>
 
 typedef struct
 {
@@ -51,6 +52,16 @@ TIM_TypeDef *TIM2 = &timer;
 #define GET_BYTES_04(msg) ((msg)->RDLR)
 #define GET_BYTES_48(msg) ((msg)->RDHR)
 
+// from board_declarations.h
+#define HW_TYPE_UNKNOWN 0U
+#define HW_TYPE_WHITE_PANDA 1U
+#define HW_TYPE_GREY_PANDA 2U
+#define HW_TYPE_BLACK_PANDA 3U
+#define HW_TYPE_PEDAL 4U
+
+// from main_declarations.h
+uint8_t hw_type = 0U;
+
 #define UNUSED(x) (void)(x)
 
 #define PANDA
@@ -90,6 +101,10 @@ int get_gas_interceptor_prev(void){
   return gas_interceptor_prev;
 }
 
+int get_hw_type(void){
+  return hw_type;
+}
+
 void set_timer(uint32_t t){
   timer.CNT = t;
 }
@@ -228,7 +243,17 @@ void set_honda_bosch_hardware(bool c){
   honda_bosch_hardware = c;
 }
 
+int get_honda_bosch_hardware(void) {
+  return honda_bosch_hardware;
+}
+
+void init_tests(void){
+  // get HW_TYPE from env variable set in test.sh
+  hw_type = atoi(getenv("HW_TYPE"));
+}
+
 void init_tests_toyota(void){
+  init_tests();
   toyota_torque_meas.min = 0;
   toyota_torque_meas.max = 0;
   toyota_desired_torque_last = 0;
@@ -238,6 +263,7 @@ void init_tests_toyota(void){
 }
 
 void init_tests_cadillac(void){
+  init_tests();
   cadillac_torque_driver.min = 0;
   cadillac_torque_driver.max = 0;
   for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = 0;
@@ -247,6 +273,7 @@ void init_tests_cadillac(void){
 }
 
 void init_tests_gm(void){
+  init_tests();
   gm_torque_driver.min = 0;
   gm_torque_driver.max = 0;
   gm_desired_torque_last = 0;
@@ -256,6 +283,7 @@ void init_tests_gm(void){
 }
 
 void init_tests_hyundai(void){
+  init_tests();
   hyundai_torque_driver.min = 0;
   hyundai_torque_driver.max = 0;
   hyundai_desired_torque_last = 0;
@@ -265,6 +293,7 @@ void init_tests_hyundai(void){
 }
 
 void init_tests_chrysler(void){
+  init_tests();
   chrysler_torque_meas.min = 0;
   chrysler_torque_meas.max = 0;
   chrysler_desired_torque_last = 0;
@@ -274,6 +303,7 @@ void init_tests_chrysler(void){
 }
 
 void init_tests_subaru(void){
+  init_tests();
   subaru_torque_driver.min = 0;
   subaru_torque_driver.max = 0;
   subaru_desired_torque_last = 0;
@@ -283,6 +313,7 @@ void init_tests_subaru(void){
 }
 
 void init_tests_honda(void){
+  init_tests();
   honda_moving = false;
   honda_brake_prev = 0;
   honda_gas_prev = 0;

panda/tests/safety/test.sh

@@ -1,2 +1,17 @@
 #!/usr/bin/env sh
-python -m unittest discover .
+
+# Loop over all hardware types:
+#   HW_TYPE_UNKNOWN 0U
+#   HW_TYPE_WHITE_PANDA 1U
+#   HW_TYPE_GREY_PANDA 2U
+#   HW_TYPE_BLACK_PANDA 3U
+#   HW_TYPE_PEDAL 4U
+
+# Make sure test fails if one HW_TYPE fails
+set -e
+
+for hw_type in 0 1 2 3 4
+do
+  echo "Testing HW_TYPE: $hw_type"
+  HW_TYPE=$hw_type python -m unittest discover .
+done

panda/tests/safety/test_honda.py

@@ -33,6 +33,10 @@ class TestHondaSafety(unittest.TestCase):
     to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
     to_send[0].RIR = msg << 21
     to_send[0].RDLR = buttons << 5
+    is_panda_black = self.safety.get_hw_type() == 3 # black_panda
+    honda_bosch_hardware = self.safety.get_honda_bosch_hardware()
+    bus = 1 if is_panda_black and honda_bosch_hardware else 0
+    to_send[0].RDTR = bus << 4
 
     return to_send
 

panda/tests/safety/test_honda_bosch.py

@@ -23,16 +23,20 @@ class TestHondaSafety(unittest.TestCase):
   def test_fwd_hook(self):
     buss = range(0x0, 0x3)
     msgs = range(0x1, 0x800)
+    is_panda_black = self.safety.get_hw_type() == 3  # black panda
+    bus_rdr_cam = 2 if is_panda_black else 1
+    bus_rdr_car = 0 if is_panda_black else 2
+    bus_pt = 1 if is_panda_black else 0
 
     blocked_msgs = [0xE4, 0x33D]
     for b in buss:
       for m in msgs:
-        if b == 0:
+        if b == bus_pt:
           fwd_bus = -1
-        elif b == 1:
-          fwd_bus = -1 if m in blocked_msgs else 2
-        elif b == 2:
-          fwd_bus = 1
+        elif b == bus_rdr_cam:
+          fwd_bus = -1 if m in blocked_msgs else bus_rdr_car
+        elif b == bus_rdr_car:
+          fwd_bus = bus_rdr_cam
 
         # assume len 8
         self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))

selfdrive/boardd/boardd.cc

@@ -59,7 +59,8 @@ pthread_mutex_t usb_lock;
 bool spoofing_started = false;
 bool fake_send = false;
 bool loopback_can = false;
-bool is_grey_panda = false;
+cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
+bool is_pigeon = false;
 
 pthread_t safety_setter_thread_handle = -1;
 pthread_t pigeon_thread_handle = -1;
@@ -69,6 +70,29 @@ void pigeon_init();
 void *pigeon_thread(void *crap);
 
 void *safety_setter_thread(void *s) {
+  char *value_vin;
+  size_t value_vin_sz = 0;
+
+  // switch to no_output when CarVin param is read
+  while (1) {
+    if (do_exit) return NULL;
+    const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz);
+    if (value_vin_sz > 0) {
+      // sanity check VIN format
+      assert(value_vin_sz == 17);
+      break;
+    }
+    usleep(100*1000);
+  }
+  LOGW("got CarVin %s", value_vin);
+
+  pthread_mutex_lock(&usb_lock);
+
+  // VIN qury done, stop listening to OBDII
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
+
+  pthread_mutex_unlock(&usb_lock);
+
   char *value;
   size_t value_sz = 0;
 
@@ -151,7 +175,7 @@ void *safety_setter_thread(void *s) {
 // must be called before threads or with mutex
 bool usb_connect() {
   int err;
-  unsigned char is_pigeon[1] = {0};
+  unsigned char hw_query[1] = {0};
 
   dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
   if (dev_handle == NULL) { goto fail; }
@@ -184,11 +208,12 @@ bool usb_connect() {
     assert(err == 0);
   }
 
-  libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, is_pigeon, 1, TIMEOUT);
+  libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT);
 
-  if (is_pigeon[0]) {
-    LOGW("grey panda detected");
-    is_grey_panda = true;
+  hw_type = (cereal::HealthData::HwType)(hw_query[0]);
+  is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA);
+  if (is_pigeon) {
+    LOGW("panda with gps detected");
     pigeon_needs_init = true;
     if (pigeon_thread_handle == -1) {
       err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
@@ -280,12 +305,13 @@ void can_health(void *s) {
   struct __attribute__((packed)) health {
     uint32_t voltage;
     uint32_t current;
-    uint8_t started;
-    uint8_t controls_allowed;
-    uint8_t gas_interceptor_detected;
     uint32_t can_send_errs;
     uint32_t can_fwd_errs;
     uint32_t gmlan_send_errs;
+    uint8_t started;
+    uint8_t controls_allowed;
+    uint8_t gas_interceptor_detected;
+    uint8_t car_harness_status_pkt;
   } health;
 
   // recv from board
@@ -293,7 +319,9 @@ void can_health(void *s) {
 
   do {
     cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT);
-    if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); }
+    if (cnt != sizeof(health)) {
+      handle_usb_issue(cnt, __func__);
+    }
   } while(cnt != sizeof(health));
 
   pthread_mutex_unlock(&usb_lock);
@@ -314,15 +342,23 @@ void can_health(void *s) {
   }
   healthData.setControlsAllowed(health.controls_allowed);
   healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
-  healthData.setIsGreyPanda(is_grey_panda);
+  healthData.setHasGps(is_pigeon);
   healthData.setCanSendErrs(health.can_send_errs);
   healthData.setCanFwdErrs(health.can_fwd_errs);
   healthData.setGmlanSendErrs(health.gmlan_send_errs);
+  healthData.setHwType(hw_type);
 
   // send to health
   auto words = capnp::messageToFlatArray(msg);
   auto bytes = words.asBytes();
   zmq_send(s, bytes.begin(), bytes.size(), 0);
+
+  pthread_mutex_lock(&usb_lock);
+
+  // send heartbeat back to panda
+  libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT);
+
+  pthread_mutex_unlock(&usb_lock);
 }
 
 
@@ -447,10 +483,10 @@ void *can_health_thread(void *crap) {
   void *publisher = zmq_socket(context, ZMQ_PUB);
   zmq_bind(publisher, "tcp://*:8011");
 
-  // run at 1hz
+  // run at 2hz
   while (!do_exit) {
     can_health(publisher);
-    usleep(1000*1000);
+    usleep(500*1000);
   }
   return NULL;
 }
@@ -502,7 +538,7 @@ void pigeon_set_baud(int baud) {
 
 void pigeon_init() {
   usleep(1000*1000);
-  LOGW("grey panda start");
+  LOGW("panda GPS start");
 
   // power off pigeon
   pigeon_set_power(0);
@@ -543,7 +579,7 @@ void pigeon_init() {
   pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
   pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
 
-  LOGW("grey panda is ready to fly");
+  LOGW("panda GPS on");
 }
 
 static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {

selfdrive/can/tests/test_packer_honda.py

@@ -16,6 +16,9 @@ class TestPackerMethods(unittest.TestCase):
   def test_correctness(self):
     # Test all commands, randomize the params.
     for _ in xrange(1000):
+      is_panda_black = False
+      car_fingerprint = HONDA_BOSCH[0]
+
       apply_brake = (random.randint(0, 2) % 2 == 0)
       pump_on = (random.randint(0, 2) % 2 == 0)
       pcm_override = (random.randint(0, 2) % 2 == 0)
@@ -23,33 +26,31 @@ class TestPackerMethods(unittest.TestCase):
       chime = random.randint(0, 65536)
       fcw = random.randint(0, 65536)
       idx = random.randint(0, 65536)
-      m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
-      m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
+      m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
+      m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
       self.assertEqual(m_old, m)
 
       apply_steer = (random.randint(0, 2) % 2 == 0)
       lkas_active = (random.randint(0, 2) % 2 == 0)
-      car_fingerprint = HONDA_BOSCH[0]
       idx = random.randint(0, 65536)
-      m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx)
-      m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx)
+      m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
+      m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
       self.assertEqual(m_old, m)
 
       pcm_speed = random.randint(0, 65536)
       hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
               0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536),
               random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
-      car_fingerprint = HONDA_BOSCH[0]
       idx = random.randint(0, 65536)
       is_metric = (random.randint(0, 2) % 2 == 0)
-      m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx)
-      m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx)
+      m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
+      m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
       self.assertEqual(m_old, m)
 
       button_val = random.randint(0, 65536)
       idx = random.randint(0, 65536)
-      m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx)
-      m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx)
+      m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black)
+      m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black)
       self.assertEqual(m_old, m)
 
 

selfdrive/car/car_helpers.py

@@ -1,13 +1,13 @@
 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
 import pickle
-from common.params import Params
-params = Params()
+import selfdrive.crash as crash
 
 
 def get_startup_alert(car_recognized, controller_available):
@@ -54,117 +54,101 @@ def _get_interface_names():
 interfaces = load_interfaces(_get_interface_names())
 
 def only_toyota_left(candidate_cars):
-  return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars)
+  return all(("TOYOTA" in c or "LEXUS" in c) for c in candidate_cars) and len(candidate_cars) > 0
 
 # BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
 # **** for use live only ****
-def fingerprint(logcan, sendcan):
+def fingerprint(logcan, sendcan, is_panda_black):
   if os.getenv("SIMULATOR2") is not None:
     return ("simulator2", None, "")
   elif os.getenv("SIMULATOR") is not None:
     return ("simulator", None, "")
 
-  finger = {0: {}, 2:{}}  # collect on bus 0 or 2
-  cloudlog.warning("waiting for fingerprint...")
-  candidate_cars = all_known_cars()
-  can_seen_frame = None
-  can_seen = False
+  params = Params()
+  car_params = params.get("CarParams")
 
-  # 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 = ""
-
-  frame = 0
+  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)
 
   if params.get("DragonCacheCar") == "1" and params.get("DragonCachedFP") != "" and params.get("DragonCachedModel") != "":
-    candidate_cars = pickle.loads(params.get("DragonCachedModel"))
+    car_fingerprint = pickle.loads(params.get("DragonCachedModel"))
     finger = pickle.loads(params.get("DragonCachedFP"))
     vin = pickle.loads(params.get("DragonCachedVIN"))
   else:
-    while True:
+    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 not done:
       a = messaging.recv_one(logcan)
 
       for can in a.can:
-        can_seen = True
-
-        # have we got a VIN query response?
-        if can.src == 0 and can.address == 0x7e8:
-          vin_never_responded = False
-          # basic sanity checks on ISO-TP response
-          if is_vin_response_valid(can.dat, vin_step, vin_cnt):
-            vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
-            vin_cnt += 1
-            if vin_cnt == vin_cnts[vin_step]:
-              vin_responded = True
-              vin_step += 1
-
-        # ignore everything not on bus 0 and with more than 11 bits,
-        # which are ussually sporadic and hard to include in fingerprints.
-        # also exclude VIN query response on 0x7e8.
+        # need to independently try to fingerprint both bus 0 and 1 to work
+        # for the combo black_panda and honda_bosch. Ignore extended messages
+        # and VIN query response.
         # Include bus 2 for toyotas to disambiguate cars using camera messages
         # (ideally should be done for all cars but we can't for Honda Bosch)
-        if (can.src == 0 or (only_toyota_left(candidate_cars) and can.src == 2)) and \
-           can.address < 0x800 and can.address != 0x7e8:
-          finger[can.src][can.address] = len(can.dat)
-          candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
+        for b in candidate_cars:
+          if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
+             can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
+            finger[can.src][can.address] = len(can.dat)
+            candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
 
-      if can_seen_frame is None and can_seen:
-        can_seen_frame = frame
+      # if we only have one car choice and the time since we got our first
+      # message has elapsed, exit
+      for b in candidate_cars:
+        # Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
+        if only_toyota_left(candidate_cars[b]):
+          frame_fingerprint = 100  # 1s
+        if len(candidate_cars[b]) == 1:
+          if frame > frame_fingerprint:
+            # fingerprint done
+            car_fingerprint = candidate_cars[b][0]
 
-      # if we only have one car choice and the time_fingerprint since we got our first
-      # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
-      # broadcast immediately
-      if len(candidate_cars) == 1 and can_seen_frame is not None:
-        time_fingerprint = 1.0 if only_toyota_left(candidate_cars) else 0.1
-        if (frame - can_seen_frame) > (time_fingerprint * 100):
-          break
-
-      # bail if no cars left or we've been waiting for more than 2s since can_seen
-      elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200):
-        return None, finger, ""
-
-      # keep sending VIN qury if ECU isn't responsing.
-      # sendcan is probably not ready due to the zmq slow joiner syndrome
-      # TODO: VIN query temporarily disabled until we have the harness
-      if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
-        sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan'))
-        vin_responded = False
-        vin_cnt = 0
+      # bail if no cars left or we've been waiting for more than 2s
+      failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200
+      succeeded = car_fingerprint is not None
+      done = failed or succeeded
 
       frame += 1
 
-    # only report vin if procedure is finished
-    if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
-      vin = "".join(vin_dat[3:])
+      if succeeded:
+        params.put("DragonCachedModel", pickle.dumps(car_fingerprint))
+        params.put("DragonCachedFP", pickle.dumps(finger))
+        params.put("DragonCachedVIN", pickle.dumps(vin))
 
-  params.put("DragonCachedModel", pickle.dumps(candidate_cars))
-  params.put("DragonCachedFP", pickle.dumps(finger))
-  params.put("DragonCachedVIN", pickle.dumps(vin))
-
-  cloudlog.warning("fingerprinted %s", candidate_cars[0])
-  cloudlog.warning("VIN %s", vin)
-  return candidate_cars[0], finger, vin
+  cloudlog.warning("fingerprinted %s", car_fingerprint)
+  return car_fingerprint, finger, vin
 
 
-def get_car(logcan, sendcan):
+def get_car(logcan, sendcan, is_panda_black=False):
 
-  candidate, fingerprints, vin = fingerprint(logcan, sendcan)
+  candidate, fingerprints, vin = fingerprint(logcan, sendcan, is_panda_black)
 
   if candidate is None:
     cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
     candidate = "mock"
+  else:
+    cloudlog.warning("car does match fingerprint: %r", fingerprints)
+    try:
+      crash.capture_warning("fingerprinted %s" % candidate)
+    except:  # fixes occasional travis errors
+      pass
 
   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

@@ -87,11 +87,19 @@ class CarController(object):
 
     # dragonpilot
     self.turning_signal_timer = 0
+    self.dragon_enable_steering_on_signal = False
+    self.dragon_allow_gas = False
+    self.dragon_lat_ctrl = True
 
   def update(self, enabled, CS, frame, actuators, \
              pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
              hud_v_cruise, hud_show_lanes, hud_show_car, \
              hud_alert, snd_beep, snd_chime):
+    # dragonpilot, don't check for param too often as it's a kernel call
+    if frame % 100 == 0:
+      self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
+      self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
+      self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
 
     # *** apply brake hysteresis ***
     brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
@@ -152,29 +160,32 @@ class CarController(object):
     can_sends = []
 
     # dragonpilot
-    if enabled and (CS.left_blinker_on > 0 or CS.right_blinker_on > 0) and params.get("DragonEnableSteeringOnSignal") == "1":
+    if enabled and (CS.left_blinker_on > 0 or CS.right_blinker_on > 0) and self.dragon_enable_steering_on_signal:
       self.turning_signal_timer = 100
 
     if self.turning_signal_timer > 0:
       self.turning_signal_timer -= 1
       lkas_active = False
 
+    if not self.dragon_lat_ctrl:
+      lkas_active = False
+
     # 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.
@@ -189,11 +200,11 @@ class CarController(object):
           gasPressed = CS.pedal_gas > 0
         else:
           gasPressed = CS.user_gas_pressed
-        if params.get("DragonAllowGas") == "1" and gasPressed:
+        if self.dragon_allow_gas and gasPressed:
           apply_brake = 0
           apply_gas = 0
         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
-
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
+  bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH  and not CP.isPandaBlack else 2
+  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
 
 class CarState(object):
   def __init__(self, CP):

selfdrive/car/honda/hondacan.py

@@ -19,7 +19,15 @@ def fix(msg, addr):
   return msg2
 
 
-def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
+def get_pt_bus(car_fingerprint, is_panda_black):
+  return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0
+
+
+def get_lkas_cmd_bus(car_fingerprint, is_panda_black):
+  return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0
+
+
+def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black):
   # TODO: do we loose pressure if we keep pump off for long?
   brakelights = apply_brake > 0
   brake_rq = apply_brake > 0
@@ -38,27 +46,25 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
     # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
     "FCW": fcw << 1,
   }
-  return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
+  bus = get_pt_bus(car_fingerprint, is_panda_black)
+  return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
 
 
-def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
+def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black):
   values = {
     "STEER_TORQUE": apply_steer if lkas_active else 0,
     "STEER_TORQUE_REQUEST": lkas_active,
   }
-  # Set bus 2 for accord and new crv.
-  bus = 2 if car_fingerprint in HONDA_BOSCH else 0
+  bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
   return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
 
 
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
+def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black):
   commands = []
-  bus = 0
+  bus_pt = get_pt_bus(car_fingerprint, is_panda_black)
+  bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
 
-  # Bosch sends commands to bus 2.
-  if car_fingerprint in HONDA_BOSCH:
-    bus = 2
-  else:
+  if car_fingerprint not in HONDA_BOSCH:
     acc_hud_values = {
       'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
       'PCM_GAS': hud.pcm_accel,
@@ -70,7 +76,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
       'SET_ME_X01_2': 1,
       'SET_ME_X01': 1,
     }
-    commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
+    commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
 
   lkas_hud_values = {
     'SET_ME_X41': 0x41,
@@ -80,23 +86,23 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
     'DASHED_LANES': hud.dashed_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

@@ -98,6 +98,11 @@ class CarInterface(object):
     else:
       self.compute_gb = compute_gb_honda
 
+    # dragonpilot
+    self.dragon_enable_steering_on_signal = False
+    self.dragon_allow_gas = False
+    self.ts_last_check = 0.
+
   @staticmethod
   def calc_accel_override(a_ego, a_target, v_ego, v_target):
 
@@ -131,12 +136,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
@@ -145,7 +151,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
 
@@ -169,7 +175,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']
@@ -189,7 +195,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.]
@@ -215,7 +221,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.]
@@ -295,7 +301,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.]
@@ -361,6 +367,13 @@ class CarInterface(object):
 
   # returns a car.CarState
   def update(self, c, can_strings):
+    # dragonpilot, don't check for param too often as it's a kernel call
+    ts = sec_since_boot()
+    if ts - self.ts_last_check > 1.:
+      self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
+      self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
+      self.ts_last_check = ts
+
     # ******************* do can recv *******************
     self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
     self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
@@ -476,7 +489,7 @@ class CarInterface(object):
       events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
     if not self.CS.lkMode:
       events.append(create_event('manualSteeringRequired', [ET.WARNING]))
-    elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1":
+    elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
       events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
     elif self.CS.steer_error:
       events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
@@ -505,7 +518,7 @@ class CarInterface(object):
       events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
 
     # DragonAllowGas
-    if params.get("DragonAllowGas") == "0":
+    if not self.dragon_allow_gas:
       # disable on pedals rising edge or when brake is pressed and speed isn't zero
       if (ret.gasPressed and not self.gas_pressed_prev) or \
          (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):

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/hyundaican.py

@@ -8,7 +8,7 @@ def make_can_msg(addr, dat, alt):
 
 def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False):
   values = {
-    "CF_Lkas_Icon": 3 if enabled else 0,
+    "CF_Lkas_Bca_R": 3 if enabled else 0,
     "CF_Lkas_LdwsSysState": 3 if steer_req else 1,
     "CF_Lkas_SysWarning": hud_alert,
     "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0,

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/carcontroller.py

@@ -128,10 +128,18 @@ class CarController(object):
 
     # dragonpilot
     self.turning_signal_timer = 0
+    self.dragon_enable_steering_on_signal = False
+    self.dragon_allow_gas = False
+    self.dragon_lat_ctrl = True
 
   def update(self, enabled, CS, frame, actuators,
              pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
+    # dragonpilot, don't check for param too often as it's a kernel call
+    if frame % 100 == 0:
+      self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
+      self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
+      self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
 
     # *** compute control surfaces ***
 
@@ -204,13 +212,27 @@ class CarController(object):
     can_sends = []
 
     # dragonpilot
-    if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1":
+    if enabled and (CS.left_blinker_on or CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
       self.turning_signal_timer = 100
 
     if self.turning_signal_timer > 0:
       self.turning_signal_timer -= 1
       apply_steer_req = 0
 
+    if not self.dragon_lat_ctrl:
+      apply_steer_req = 0
+
+    if CS.v_ego > 12.5 and not enabled:
+      if right_lane_depart and not CS.right_blinker_on:
+        apply_steer = self.last_steer + 3
+        apply_steer = min(apply_steer , 800)
+        apply_steer_req = 1
+
+      if left_lane_depart and not CS.left_blinker_on:
+        apply_steer = self.last_steer - 3
+        apply_steer = max(apply_steer , -800)
+        apply_steer_req = 1
+
     #*** control msgs ***
     #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
 
@@ -238,7 +260,7 @@ class CarController(object):
     else:
       gasPressed = CS.pedal_gas > 0
 
-    if params.get("DragonAllowGas") == "1" and gasPressed:
+    if self.dragon_allow_gas and gasPressed:
       apply_accel = 0
       apply_gas = 0
 

selfdrive/car/toyota/carstate.py

@@ -3,7 +3,11 @@ 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
+
+from common.realtime import sec_since_boot
+from common.params import Params
+params = Params()
 
 def parse_gear_shifter(gear, vals):
 
@@ -19,6 +23,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 +64,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 +97,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
@@ -108,7 +113,16 @@ class CarState(object):
                          K=[[0.12287673], [0.29666309]])
     self.v_ego = 0.0
 
+    self.dragon_toyota_stock_dsu = False
+    self.ts_last_check = 0.
+
   def update(self, cp):
+    # dragonpilot, don't check for param too often as it's a kernel call
+    ts = sec_since_boot()
+    if ts - self.ts_last_check > 1.:
+      self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
+      self.ts_last_check = ts
+
     # update prevs, update must run once per loop
     self.prev_left_blinker_on = self.left_blinker_on
     self.prev_right_blinker_on = self.right_blinker_on
@@ -144,6 +158,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']
@@ -173,3 +195,13 @@ class CarState(object):
       self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
     else:
       self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
+
+    if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on:
+      enable_acc = True
+      if not self.gear_shifter == 'drive' or not self.seatbelt or not self.door_all_closed:
+        enable_acc = False
+      self.pcm_acc_active = enable_acc
+      if self.standstill:
+        self.pcm_acc_status = 7
+      else:
+        self.pcm_acc_status = 1

selfdrive/car/toyota/interface.py

@@ -33,6 +33,12 @@ class CarInterface(object):
     if CarController is not None:
       self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
 
+    # dragonpilot
+    self.dragon_toyota_stock_dsu = False
+    self.dragon_enable_steering_on_signal = False
+    self.dragon_allow_gas = False
+    self.ts_last_check = 0.
+
   @staticmethod
   def compute_gb(accel, speed):
     return float(accel) / 3.0
@@ -42,13 +48,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
 
@@ -64,7 +71,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
 
@@ -80,7 +87,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]]
@@ -90,7 +97,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]]
@@ -215,7 +222,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
@@ -248,6 +255,14 @@ class CarInterface(object):
 
   # returns a car.CarState
   def update(self, c, can_strings):
+    # dragonpilot, don't check for param too often as it's a kernel call
+    ts = sec_since_boot()
+    if ts - self.ts_last_check > 1.:
+      self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
+      self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
+      self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
+      self.ts_last_check = ts
+
     # ******************* do can recv *******************
     self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
 
@@ -349,7 +364,7 @@ class CarInterface(object):
       events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
     if ret.gearShifter == 'reverse' and self.CP.enableDsu:
       events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
-    if (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1":
+    if (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
       events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
     elif self.CS.steer_error:
       events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
@@ -370,18 +385,19 @@ class CarInterface(object):
     elif not ret.cruiseState.enabled:
       events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
 
-    # DragonAllowGas
-    if params.get("DragonAllowGas") == "0":
-      # disable on pedals rising edge or when brake is pressed and speed isn't zero
-      if (ret.gasPressed and not self.gas_pressed_prev) or \
-         (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
-        events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+    if not self.dragon_toyota_stock_dsu:
+      # DragonAllowGas
+      if not self.dragon_allow_gas:
+        # disable on pedals rising edge or when brake is pressed and speed isn't zero
+        if (ret.gasPressed and not self.gas_pressed_prev) or \
+           (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
+          events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
-      if ret.gasPressed:
-        events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
-    else:
-      if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001):
-        events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+        if ret.gasPressed:
+          events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+      else:
+        if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001):
+          events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
     ret.events = events
 

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

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import os
 import gc
 import capnp
 from cereal import car, log
@@ -48,6 +49,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 +363,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 +417,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,14 +431,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()
+  can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
+  can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
 
   car_recognized = CP.carName != 'mock'
   # If stock camera is disconnected, we loaded car controls and it's not chffrplus
@@ -443,6 +451,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 +471,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,13 +484,24 @@ 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)
 
   prof = Profiler(False)  # off by default
 
+  # dragonpilot
+  ts_last_check = 0.
+  dragon_toyota_stock_dsu = False
+
   while True:
+    # dragonpilot, don't check for param too often as it's a kernel call
+    ts = sec_since_boot()
+    if ts - ts_last_check > 1.:
+      dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
+      ts_last_check = ts
+
     start_time = sec_since_boot()
     prof.checkpoint("Ratekeeper", ignore=True)
 
@@ -498,6 +520,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:
@@ -505,9 +529,10 @@ def controlsd_thread(gctx=None):
     if not CS.canValid:
       events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
-    # Only allow engagement with brake pressed when stopped behind another stopped car
-    if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
-      events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if not dragon_toyota_stock_dsu:
+      # Only allow engagement with brake pressed when stopped behind another stopped car
+      if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
+        events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
     if not read_only:
       # update control state

selfdrive/controls/lib/alerts.py

@@ -385,6 +385,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",
@@ -542,6 +549,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

@@ -12,6 +12,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
@@ -25,16 +26,15 @@ MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
 RESIZED_FOCAL = 320.0
 H, W, FULL_W = 320, 160, 426
 
-
-def head_orientation_from_descriptor(desc):
+def head_orientation_from_descriptor(angles_desc, pos_desc):
   # the output of these angles are in device frame
   # so from driver's perspective, pitch is up and yaw is right
   # TODO this should be calibrated
-  pitch_prnet = desc[0]
-  yaw_prnet = desc[1]
-  roll_prnet = desc[2]
+  pitch_prnet = angles_desc[0]
+  yaw_prnet = angles_desc[1]
+  roll_prnet = angles_desc[2]
 
-  face_pixel_position = ((desc[3] + .5)*W - W + FULL_W, (desc[4]+.5)*H)
+  face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
   yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL)
   pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL)
 
@@ -74,6 +74,10 @@ class DriverStatus():
     self.terminal_alert_cnt = 0
     self._set_timers()
 
+    # dragonpilot
+    self.dp_last_check = 0.
+    self.dragon_enable_driver_safety_check = True
+
   def _reset_filters(self):
     self.driver_distraction_filter.x = 0.
     self.variance_filter.x = 0.
@@ -98,24 +102,22 @@ class DriverStatus():
       pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
     pitch_error *= _PITCH_WEIGHT
     metric = np.sqrt(yaw_error**2 + pitch_error**2)
-    #print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric
+    # TODO: do something with the eye states and combine them with head pose
     return 1 if metric > _METRIC_THRESHOLD else 0
 
 
   def get_pose(self, driver_monitoring, params):
+    if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
+      return
 
-    self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.descriptor)
-
-    # TODO: DM data should not be in a list if they are not homogeneous
-    if len(driver_monitoring.descriptor) > 6:
-      self.face_detected = driver_monitoring.descriptor[6] > 0.
-    else:
-      self.face_detected = True
+    self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition)
+    self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
 
     self.driver_distracted = self._is_driver_distracted(self.pose)
     # first order filters
     self.driver_distraction_filter.update(self.driver_distracted)
-    self.variance_high = driver_monitoring.std > _STD_THRESHOLD
+    self.variance_high = False #driver_monitoring.std > _STD_THRESHOLD
+
     self.variance_filter.update(self.variance_high)
 
     monitor_param_on_prev = self.monitor_param_on
@@ -135,6 +137,11 @@ class DriverStatus():
 
 
   def update(self, events, driver_engaged, ctrl_active, standstill):
+    # don't check for param too often as it's a kernel call
+    ts = sec_since_boot()
+    if ts - self.dp_last_check > 1.:
+      self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck") == "0" else True
+      self.dp_last_check = ts
 
     driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
     awareness_prev = self.awareness
@@ -160,7 +167,7 @@ class DriverStatus():
     elif self.awareness <= self.threshold_pre:
       # pre green alert
       alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
-    if alert is not None and params.get("DragonEnableDriverSafetyCheck") == "1":
+    if alert is not None and self.dragon_enable_driver_safety_check:
       events.append(create_event(alert, [ET.WARNING]))
 
     return events

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
+  return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
 class Planner(object):
@@ -79,16 +89,21 @@ class Planner(object):
     self.a_acc = 0.0
     self.v_cruise = 0.0
     self.a_cruise = 0.0
+    self.v_model = 0.0
+    self.a_model = 0.0
 
     self.longitudinalPlanSource = 'cruise'
     self.fcw_checker = FCWChecker()
     self.fcw_enabled = fcw_enabled
 
+    self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
+    self.model_v_kf_ready = False
+
     self.params = Params()
 
   def choose_solution(self, v_cruise_setpoint, enabled):
     if enabled:
-      solutions = {'cruise': self.v_cruise}
+      solutions = {'cruise': self.v_cruise, 'model': self.v_model}
       if self.mpc1.prev_lead_status:
         solutions['mpc1'] = self.mpc1.v_mpc
       if self.mpc2.prev_lead_status:
@@ -108,10 +123,13 @@ class Planner(object):
       elif slowest == 'cruise':
         self.v_acc = self.v_cruise
         self.a_acc = self.a_cruise
+      elif slowest == 'model':
+        self.v_acc = self.v_model
+        self.a_acc = self.a_model
 
     self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
 
-  def update(self, sm, CP, VM, PP, live_map_data):
+  def update(self, sm, CP, VM, PP):
     """Gets called when new radarState is available"""
     cur_time = sec_since_boot()
     v_ego = sm['carState'].vEgo
@@ -127,51 +145,43 @@ class Planner(object):
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
 
-    v_speedlimit = NO_CURVATURE_SPEED
-    v_curvature = NO_CURVATURE_SPEED
+    if not self.model_v_kf_ready:
+      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
+    if len(sm['model'].speed):
+      self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
 
-    # 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:
-        curvature = abs(live_map_data.liveMapData.curvature)
-        a_y_max = 2.975 - v_ego * 0.0375  # ~1.85 @ 75mph, ~2.6 @ 25mph
-        v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
-        v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
-
-    decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
-    v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
+    if self.params.get("LimitSetSpeedNeural") == "1":
+      model_speed = self.model_v_kf.x[0][0]
+    else:
+      model_speed = MAX_SPEED
 
     # Calculate speed for normal cruise control
     if enabled:
       accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
       jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]  # TODO: make a separate lookup for jerk tuning
-      accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
+      accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
 
       if force_slow_decel:
         # if required so, force a smooth deceleration
-        accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
-        accel_limits[0] = min(accel_limits[0], accel_limits[1])
-
-      # Change accel limits based on time remaining to turn
-      if decel_for_turn:
-        time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
-        required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
-        accel_limits[0] = max(accel_limits[0], required_decel)
+        accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
+        accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
 
       self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
                                                     v_cruise_setpoint,
-                                                    accel_limits[1], accel_limits[0],
+                                                    accel_limits_turns[1], accel_limits_turns[0],
                                                     jerk_limits[1], jerk_limits[0],
                                                     LON_MPC_STEP)
+
+      # accel and jerk up limits are higher here to make model not limiting accel
+      # mainly done to prevent flickering of slowdown icon
+      self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
+                                                    model_speed,
+                                                    2*accel_limits[1], 3*accel_limits[0],
+                                                    2*jerk_limits[1], jerk_limits[0],
+                                                    LON_MPC_STEP)
+
       # cruise speed can't be negative even is user is distracted
       self.v_cruise = max(self.v_cruise, 0.)
     else:
@@ -234,10 +244,6 @@ class Planner(object):
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 
-    plan_send.plan.vCurvature = v_curvature
-    plan_send.plan.decelForTurn = decel_for_turn
-    plan_send.plan.mapValid = map_valid
-
     radar_valid = not (radar_dead or radar_fault)
     plan_send.plan.radarValid = bool(radar_valid)
     plan_send.plan.radarCanError = bool(radar_can_error)

selfdrive/controls/lib/radar_helpers.py

@@ -1,48 +1,35 @@
-import numpy as np
-
-from common.numpy_fast import clip, interp
+from common.realtime import DT_MDL
 from common.kalman.simple_kalman import KF1D
+from selfdrive.config import RADAR_TO_CENTER
 
+
+# the longer lead decels, the more likely it will keep decelerating
+# TODO is this a good default?
 _LEAD_ACCEL_TAU = 1.5
-NO_FUSION_SCORE = 100 # bad default fusion score
 
 # radar tracks
 SPEED, ACCEL = 0, 1   # Kalman filter states enum
 
-rate, ratev = 20., 20.    # model and radar are both at 20Hz
-ts = 1./rate
-freq_v_lat = 0.2 # Hz
-k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
-
-freq_a_lead = .5 # Hz
-k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
-
 # stationary qualification parameters
-v_stationary_thr = 4.   # objects moving below this speed are classified as stationary
-v_oncoming_thr   = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
 v_ego_stationary = 4.   # no stationary object flag below this speed
 
 # Lead Kalman Filter params
-_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
+_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]]
 _VLEAD_C = [1.0, 0.0]
 #_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
 #_VLEAD_R = 1e3
 #_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
-_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]]
-
-RDR_TO_LDR = 2.7
+_VLEAD_K = [[0.1988689], [0.28555364]]
 
 
 class Track(object):
   def __init__(self):
     self.ekf = None
-    self.stationary = True
     self.initted = False
 
-  def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
+  def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
     if self.initted:
       # pylint: disable=access-member-before-definition
-      self.dPathPrev = self.dPath
       self.vLeadPrev = self.vLead
       self.vRelPrev = self.vRel
 
@@ -52,9 +39,6 @@ class Track(object):
     self.vRel = v_rel   # REL_SPEED
     self.measured = measured   # measured or estimate
 
-    # compute distance to path
-    self.dPath = d_path
-
     # computed velocity and accelerations
     self.vLead = self.vRel + v_ego_t_aligned
 
@@ -64,21 +48,8 @@ class Track(object):
       self.cnt = 1
       self.vision_cnt = 0
       self.vision = False
-      self.aRel = 0.      # nidec gives no information about this
-      self.vLat = 0.
       self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
     else:
-      # estimate acceleration
-      # TODO: use Kalman filter
-      a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
-      a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
-      self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
-
-      # TODO: use Kalman filter
-      # neglect steer override cases as dPath is too noisy
-      v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
-      self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
-
       self.kf.update(self.vLead)
 
       self.cnt += 1
@@ -86,33 +57,12 @@ class Track(object):
     self.vLeadK = float(self.kf.x[SPEED][0])
     self.aLeadK = float(self.kf.x[ACCEL][0])
 
-    if self.stationary:
-      # stationary objects can become non stationary, but not the other way around
-      self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
-    self.oncoming = self.vLead < v_oncoming_thr
-
-    self.vision_score = NO_FUSION_SCORE
-
     # Learn if constant acceleration
     if abs(self.aLeadK) < 0.5:
       self.aLeadTau = _LEAD_ACCEL_TAU
     else:
       self.aLeadTau *= 0.9
 
-  def update_vision_score(self, dist_to_vision, rel_speed_diff):
-    # rel speed is very hard to estimate from vision
-    if dist_to_vision < 4.0 and rel_speed_diff < 10.:
-      self.vision_score = dist_to_vision + rel_speed_diff
-    else:
-      self.vision_score = NO_FUSION_SCORE
-
-  def update_vision_fusion(self):
-    # vision point is never stationary
-    # don't trust 1 or 2 fusions until model quality is much better
-    if self.vision_cnt >= 3:
-      self.vision = True
-      self.stationary = False
-
   def get_key_for_cluster(self):
     # Weigh y higher since radar is inaccurate in this dimension
     return [self.dRel, self.yRel*2, self.vRel]
@@ -171,89 +121,47 @@ class Cluster(object):
   def aLeadTau(self):
     return mean([t.aLeadTau for t in self.tracks])
 
-  @property
-  def vision(self):
-    return any([t.vision for t in self.tracks])
-
   @property
   def measured(self):
     return any([t.measured for t in self.tracks])
 
-  @property
-  def vision_cnt(self):
-    return max([t.vision_cnt for t in self.tracks])
-
-  @property
-  def stationary(self):
-    return all([t.stationary for t in self.tracks])
-
-  @property
-  def oncoming(self):
-    return all([t.oncoming for t in self.tracks])
-
-  def toRadarState(self):
+  def get_RadarState(self, model_prob=0.0):
     return {
-      "dRel": float(self.dRel) - RDR_TO_LDR,
+      "dRel": float(self.dRel),
       "yRel": float(self.yRel),
       "vRel": float(self.vRel),
-      "aRel": float(self.aRel),
       "vLead": float(self.vLead),
-      "dPath": float(self.dPath),
-      "vLat": float(self.vLat),
       "vLeadK": float(self.vLeadK),
       "aLeadK": float(self.aLeadK),
       "status": True,
-      "fcw": self.is_potential_fcw(),
+      "fcw": self.is_potential_fcw(model_prob),
+      "modelProb": model_prob,
+      "radar": True,
       "aLeadTau": float(self.aLeadTau)
     }
 
+  def get_RadarState_from_vision(self, lead_msg, v_ego):
+    return {
+      "dRel": float(lead_msg.dist - RADAR_TO_CENTER),
+      "yRel": float(lead_msg.relY),
+      "vRel": float(lead_msg.relVel),
+      "vLead": float(v_ego + lead_msg.relVel),
+      "vLeadK": float(v_ego + lead_msg.relVel),
+      "aLeadK": float(0),
+      "aLeadTau": _LEAD_ACCEL_TAU,
+      "fcw": False,
+      "modelProb": float(lead_msg.prob),
+      "radar": False,
+      "status": True
+    }
+
   def __str__(self):
-    ret = "x: %4.1f  y: %4.1f  v: %4.1f  a: %4.1f  d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath)
-    if self.stationary:
-      ret += " stationary"
-    if self.vision:
-      ret += " vision"
-    if self.oncoming:
-      ret += " oncoming"
-    if self.vision_cnt > 0:
-      ret += " vision_cnt: %6.0f" % self.vision_cnt
+    ret = "x: %4.1f  y: %4.1f  v: %4.1f  a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
     return ret
 
-  def is_potential_lead(self, v_ego):
-    # predict cut-ins by extrapolating lateral speed by a lookahead time
-    # lookahead time depends on cut-in distance. more attentive for close cut-ins
-    # also, above 50 meters the predicted path isn't very reliable
+  def potential_low_speed_lead(self, v_ego):
+    # stop for stuff in front of you and low speed, even without model confirmation
+    return abs(self.yRel) < 1.5 and (v_ego < v_ego_stationary) and self.dRel < 25
 
-    # 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):
-    # is this cluster trustrable enough for triggering fcw?
-    # fcw can trigger only on clusters that have been fused vision model for at least 20 frames
-    return self.vision_cnt >= 20
+  def is_potential_fcw(self, model_prob):
+    return model_prob > .9

selfdrive/controls/plannerd.py

@@ -37,8 +37,6 @@ def plannerd_thread():
   sm['liveParameters'].sensorValid = True
   sm['liveParameters'].steerRatio = CP.steerRatio
   sm['liveParameters'].stiffnessFactor = 1.0
-  live_map_data = messaging.new_message()
-  live_map_data.init('liveMapData')
 
   while True:
     sm.update()
@@ -46,9 +44,7 @@ def plannerd_thread():
     if sm.updated['model']:
       PP.update(sm, CP, VM)
     if sm.updated['radarState']:
-      PL.update(sm, CP, VM, PP, live_map_data.liveMapData)
-    # elif socket is live_map_data_sock:
-    #   live_map_data = msg
+      PL.update(sm, CP, VM, PP)
 
 
 def main(gctx=None):

selfdrive/controls/radard.py

@@ -6,18 +6,13 @@ from collections import defaultdict, deque
 
 import selfdrive.messaging as messaging
 from selfdrive.services import service_list
-from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
-from selfdrive.controls.lib.model_parser import ModelParser
-from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
-                                                 RDR_TO_LDR, NO_FUSION_SCORE
-
+from selfdrive.controls.lib.radar_helpers import Track, Cluster
+from selfdrive.config import RADAR_TO_CENTER
 from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
-from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.swaglog import cloudlog
 from cereal import car
 from common.params import Params
 from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL
-from common.kalman.ekf import EKF, SimpleSensor
 
 DEBUG = False
 
@@ -26,106 +21,94 @@ DIMSV = 2
 XV, SPEEDV = 0, 1
 VISION_POINT = -1
 
-path_x = np.arange(0.0, 140.0, 0.1)    # 140 meters is max
-
 # Time-alignment
 rate = 1. / DT_MDL  # model and radar are both at 20Hz
 v_len = 20   # how many speed data points to remember for t alignment with rdr data
 
-class EKFV1D(EKF):
-  def __init__(self):
-    super(EKFV1D, self).__init__(False)
-    self.identity = numpy.matlib.identity(DIMSV)
-    self.state = np.matlib.zeros((DIMSV, 1))
-    self.var_init = 1e2   # ~ model variance when probability is 70%, so good starting point
-    self.covar = self.identity * self.var_init
 
-    self.process_noise = np.matlib.diag([0.5, 1])
+def laplacian_cdf(x, mu, b):
+  b = np.max([b, 1e-4])
+  return np.exp(-abs(x-mu)/b)
+
+
+def match_vision_to_cluster(v_ego, lead, clusters):
+  # match vision point to best statistical cluster match
+  probs = []
+  offset_vision_dist = lead.dist - RADAR_TO_CENTER
+  for c in clusters:
+    prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
+    prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
+    prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
+    # This is isn't exactly right, but good heuristic
+    combined_prob = prob_d * prob_y * prob_v
+    probs.append(combined_prob)
+  idx = np.argmax(probs)
+  # if no 'sane' match is found return -1
+  # stationary radar points can be false positives
+  dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+  vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2)
+  if dist_sane and vel_sane:
+    return idx
+  else:
+    return None
+
+
+def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
+  # Determine leads, this is where the essential logic happens
+  if len(clusters) > 0 and ready and lead_msg.prob > .5:
+    lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters)
+  else:
+    lead_idx = None
+
+  lead_dict = {'status': False}
+  if lead_idx is not None:
+    lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob)
+  elif (lead_idx is None) and ready and (lead_msg.prob > .5):
+    lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
+
+  if low_speed_override:
+    low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
+    if len(low_speed_clusters) > 0:
+      lead_idx = np.argmin([c.dRel for c in low_speed_clusters])
+      if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']):
+        lead_dict = low_speed_clusters[lead_idx].get_RadarState()
+
+  return lead_dict
 
-  def calc_transfer_fun(self, dt):
-    tf = np.matlib.identity(DIMSV)
-    tf[XV, SPEEDV] = dt
-    tfj = tf
-    return tf, tfj
 
 class RadarD(object):
-  def __init__(self, VM, mocked):
-    self.VM = VM
+  def __init__(self, mocked):
+    self.current_time = 0
     self.mocked = mocked
 
-    self.MP = ModelParser()
     self.tracks = defaultdict(dict)
 
     self.last_md_ts = 0
     self.last_controls_state_ts = 0
 
     self.active = 0
-    self.steer_angle = 0.
-    self.steer_override = False
-
-    # Kalman filter stuff:
-    self.ekfv = EKFV1D()
-    self.speedSensorV = SimpleSensor(XV, 1, 2)
 
     # v_ego
     self.v_ego = 0.
     self.v_ego_hist_t = deque([0], maxlen=v_len)
     self.v_ego_hist_v = deque([0], maxlen=v_len)
     self.v_ego_t_aligned = 0.
+    self.ready = False
 
-  def update(self, frame, delay, sm, rr):
-    ar_pts = {}
-    for pt in rr.points:
-      ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
-
-    if sm.updated['liveParameters']:
-      self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
+  def update(self, frame, delay, sm, rr, has_radar):
+    self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
 
     if sm.updated['controlsState']:
       self.active = sm['controlsState'].active
       self.v_ego = sm['controlsState'].vEgo
-      self.steer_angle = sm['controlsState'].angleSteers
-      self.steer_override = sm['controlsState'].steerOverride
-
       self.v_ego_hist_v.append(self.v_ego)
       self.v_ego_hist_t.append(float(frame)/rate)
-
-      self.last_controls_state_ts = sm.logMonoTime['controlsState']
-
     if sm.updated['model']:
-      self.last_md_ts = sm.logMonoTime['model']
-      self.MP.update(self.v_ego, sm['model'])
+      self.ready = True
 
-    # 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 ***
-    if (self.active and not self.steer_override) or self.mocked:
-      # use path from model (always when mocking as steering is too noisy)
-      path_y = np.polyval(self.MP.d_poly, path_x)
-    else:
-      # use path from steer, set angle_offset to 0 it does not only report the physical offset
-      path_y = calc_lookahead_offset(self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0]
+    ar_pts = {}
+    for pt in rr.points:
+      ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
 
     # *** remove missing points from meta data ***
     for ids in self.tracks.keys():
@@ -134,48 +117,21 @@ class RadarD(object):
 
     # *** compute the tracks ***
     for ids in ar_pts:
-      # ignore standalone vision point, unless we are mocking the radar
-      if ids == VISION_POINT and not self.mocked:
-        continue
       rpt = ar_pts[ids]
 
       # align v_ego by a fixed time to align it with the radar measurement
       cur_time = float(frame)/rate
       self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v)
 
-      d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
-      # add sign
-      d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
-
       # create the track if it doesn't exist or it's a new track
       if ids not in self.tracks:
         self.tracks[ids] = Track()
-      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override)
-
-    # allow the vision model to remove the stationary flag if distance and rel speed roughly match
-    if VISION_POINT in ar_pts:
-      fused_id = None
-      best_score = NO_FUSION_SCORE
-      for ids in self.tracks:
-        dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - self.tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - self.tracks[ids].yRel)) ** 2)
-        rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel)
-        self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff)
-        if best_score > self.tracks[ids].vision_score:
-          fused_id = ids
-          best_score = self.tracks[ids].vision_score
-
-      if fused_id is not None:
-        self.tracks[fused_id].vision_cnt += 1
-        self.tracks[fused_id].update_vision_fusion()
-
-    if DEBUG:
-      print("NEW CYCLE")
-      if VISION_POINT in ar_pts:
-        print("vision", ar_pts[VISION_POINT])
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3])
 
     idens = list(self.tracks.keys())
     track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens])
 
+
     # If we have multiple points, cluster them
     if len(track_pts) > 1:
       cluster_idxs = cluster_points_centroid(track_pts, 2.5)
@@ -186,29 +142,12 @@ class RadarD(object):
         if clusters[cluster_i] is None:
           clusters[cluster_i] = Cluster()
         clusters[cluster_i].add(self.tracks[idens[idx]])
-
     elif len(track_pts) == 1:
-      # TODO: why do we need this?
       clusters = [Cluster()]
       clusters[0].add(self.tracks[idens[0]])
     else:
       clusters = []
 
-    if DEBUG:
-      for i in clusters:
-        print(i)
-    # *** extract the lead car ***
-    lead_clusters = [c for c in clusters
-                     if c.is_potential_lead(self.v_ego)]
-    lead_clusters.sort(key=lambda x: x.dRel)
-    lead_len = len(lead_clusters)
-
-    # *** extract the second lead from the whole set of leads ***
-    lead2_clusters = [c for c in lead_clusters
-                      if c.is_potential_lead2(lead_clusters)]
-    lead2_clusters.sort(key=lambda x: x.dRel)
-    lead2_len = len(lead2_clusters)
-
     # *** publish radarState ***
     dat = messaging.new_message()
     dat.init('radarState')
@@ -217,18 +156,14 @@ class RadarD(object):
     dat.radarState.canMonoTimes = list(rr.canMonoTimes)
     dat.radarState.radarErrors = list(rr.errors)
     dat.radarState.controlsStateMonoTime = self.last_controls_state_ts
-    if lead_len > 0:
-      dat.radarState.leadOne = lead_clusters[0].toRadarState()
-      if lead2_len > 0:
-        dat.radarState.leadTwo = lead2_clusters[0].toRadarState()
-      else:
-        dat.radarState.leadTwo.status = False
-    else:
-      dat.radarState.leadOne.status = False
 
+    if has_radar:
+      dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
+      dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
     return dat
 
-## fuses camera and radar data for best lead detection
+
+# fuses camera and radar data for best lead detection
 def radard_thread(gctx=None):
   set_realtime_priority(2)
 
@@ -236,7 +171,6 @@ def radard_thread(gctx=None):
   cloudlog.info("radard is waiting for CarParams")
   CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
   mocked = CP.carName == "mock"
-  VM = VehicleModel(CP)
   cloudlog.info("radard got CarParams")
 
   # import the radar from the fingerprint
@@ -253,7 +187,9 @@ def radard_thread(gctx=None):
   liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
 
   rk = Ratekeeper(rate, print_delay_threshold=None)
-  RD = RadarD(VM, mocked)
+  RD = RadarD(mocked)
+
+  has_radar = not CP.radarOffCan
 
   while 1:
     can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
@@ -264,7 +200,7 @@ def radard_thread(gctx=None):
 
     sm.update(0)
 
-    dat = RD.update(rk.frame, RI.delay, sm, rr)
+    dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
     dat.radarState.cumLagMs = -rk.remaining*1000.
 
     radarState.send(dat.to_bytes())
@@ -275,29 +211,20 @@ def radard_thread(gctx=None):
     dat.init('liveTracks', len(tracks))
 
     for cnt, ids in enumerate(tracks.keys()):
-      if DEBUG:
-        print("id: %4.0f x:  %4.1f  y: %4.1f  vr: %4.1f d: %4.1f  va: %4.1f  vl: %4.1f  vlk: %4.1f alk: %4.1f  s: %1.0f  v: %1.0f" % \
-          (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
-           tracks[ids].dPath, tracks[ids].vLat,
-           tracks[ids].vLead, tracks[ids].vLeadK,
-           tracks[ids].aLeadK,
-           tracks[ids].stationary,
-           tracks[ids].measured))
       dat.liveTracks[cnt] = {
         "trackId": ids,
         "dRel": float(tracks[ids].dRel),
         "yRel": float(tracks[ids].yRel),
         "vRel": float(tracks[ids].vRel),
-        "aRel": float(tracks[ids].aRel),
-        "stationary": bool(tracks[ids].stationary),
-        "oncoming": bool(tracks[ids].oncoming),
       }
     liveTracks.send(dat.to_bytes())
 
     rk.monitor_time()
 
+
 def main(gctx=None):
   radard_thread(gctx)
 
+
 if __name__ == "__main__":
   main()

selfdrive/dragonpilot/dragonconf/__init__.py

@@ -1,10 +1,5 @@
 #!/usr/bin/env python2.7
-import os
 from common.params import Params
-# import json
-
-# file = '/data/dragonpilot.json'
-
 
 default_conf = {
   'DragonEnableDashcam': '1',
@@ -18,12 +13,19 @@ default_conf = {
   'DragonDisableUploader': '0', # deprecated
   'DragonEnableUploader': '1',
   'DragonNoctuaMode': '0',
-  'DragonCacheCar': '1',
+  'DragonCacheCar': '0',
   'DragonCachedModel': '', # for cache car
   'DragonCachedFP': '', # for cache car
   'DragonCachedVIN': '', # for cache car
   'DragonAllowGas': '0',
-  'DragonBBUI': '0',
+  'DragonBBUI': '0', # deprecated
+  'DragonToyotaStockDSU': '0',
+  'DragonLatCtrl': '1',
+  'DragonUIEvent': '0',
+  'DragonUIMaxSpeed': '0',
+  'DragonUIFace': '0',
+  'DragonUIDev': '0',
+  'DragonUIDevMini': '1',
 }
 
 deprecated_conf = {
@@ -31,6 +33,7 @@ deprecated_conf = {
   'DragonTempDisableSteerOnSignal': 'DragonEnableSteeringOnSignal',
   'DragonDisableLogger': 'DragonEnableLogger',
   'DragonDisableUploader': 'DragonEnableUploader',
+  'DragonBBUI': 'DragonUIDev',
 }
 
 deprecated_conf_invert = {
@@ -38,18 +41,20 @@ deprecated_conf_invert = {
   'DragonTempDisableSteerOnSignal': False,
   'DragonDisableLogger': True,
   'DragonDisableUploader': True,
+  'DragonBBUI': False
 }
 
 def dragonpilot_set_params(params):
   # remove deprecated params
   for old, new in deprecated_conf.items():
     if params.get(old) is not None:
-      old_val = str(params.get(old))
-      new_val = old_val
-      # invert the value if true
-      if old in deprecated_conf_invert and deprecated_conf_invert[old] is True:
-        new_val = "1" if old_val == "0" else "0"
-      params.put(new, new_val)
+      if new is not None:
+        old_val = str(params.get(old))
+        new_val = old_val
+        # invert the value if true
+        if old in deprecated_conf_invert and deprecated_conf_invert[old] is True:
+          new_val = "1" if old_val == "0" else "0"
+        params.put(new, new_val)
       params.delete(old)
 
   # set params

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) {
-  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>();
+void Localizer::handle_log(cereal::Event::Reader event) {
   double current_time = event.getLogMonoTime() / 1.0e9;
 
   if (prev_update_time < 0) {
@@ -89,8 +91,6 @@ cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t
   default:
     break;
   }
-
-  return type;
 }
 
 
@@ -101,8 +101,12 @@ extern "C" {
   }
 
   void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) {
+    const kj::ArrayPtr<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

@@ -137,6 +137,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',
@@ -267,6 +270,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()
 
@@ -374,7 +379,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
@@ -572,6 +576,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")
 
   dragonpilot_set_params(params)
 

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)
-      if controls_state:
-        last_controls_state = controls_state[-1]
+      # distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
+      log = plant.step(speed_lead, current_button, grade)
 
-      d_rel = distance_lead - distance if self.lead_relevancy else 200.
-      v_rel = speed_lead - speed if self.lead_relevancy else 0.
+      if log['controls_state_msgs']:
+        last_controls_state = log['controls_state_msgs'][-1]
+
+      d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
+      v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
+      log['d_rel'] = d_rel
+      log['v_rel'] = v_rel
 
       if last_controls_state:
         # print(last_controls_state)
         #develop plots
         plot.add_data(
           time=plant.current_time(),
-          gas=gas, brake=brake, steer_torque=steer_torque,
-          distance=distance, speed=speed, acceleration=acceleration,
+          gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
+          distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
           up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
           uf_accel_cmd=last_controls_state.ufAccelCmd,
           d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
@@ -63,10 +70,17 @@ class Maneuver(object):
           cruise_speed=last_controls_state.vCruise,
           jerk_factor=last_controls_state.jerkFactor,
           a_target=last_controls_state.aTarget,
-          fcw=fcw)
+          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.std = 0.1
+      md.model.lead.prob = prob
+      md.model.lead.relY = 0.0
+      md.model.lead.relYStd = 1.
+      md.model.lead.relVel = float(v_rel)
+      md.model.lead.relVelStd = 1.
+      md.model.lead.relA = 0.0
+      md.model.lead.relAStd = 10.
+      md.model.lead.std = 1.0
+
       cal.liveCalibration.calStatus = 1
       cal.liveCalibration.calPerc = 100
       # fake values?
@@ -383,7 +402,17 @@ class Plant(object):
     self.distance_lead_prev = distance_lead
 
     self.rk.keep_time()
-    return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs)
+    return {
+      "distance": distance,
+      "speed": speed,
+      "acceleration": acceleration,
+      "distance_lead": distance_lead,
+      "brake": brake,
+      "gas": gas,
+      "steer_torque": steer_torque,
+      "fcw": fcw,
+      "controls_state_msgs": controls_state_msgs,
+    }
 
 # simple engage in standalone mode
 def plant_thread(rate=100):

selfdrive/test/tests/plant/test_longitudinal.py

@@ -22,22 +22,34 @@ def create_dir(path):
   except OSError:
     pass
 
+
+def check_no_collision(log):
+  return min(log['d_rel']) > 0
+
+def check_fcw(log):
+  return any(log['fcw'])
+
+def check_engaged(log):
+  return log['controls_state_msgs'][-1][-1].active
+
 maneuvers = [
   Maneuver(
     'while cruising at 40 mph, change cruise speed to 50mph',
     duration=30.,
     initial_speed = 40. * CV.MPH_TO_MS,
     cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
-                            (CB.RES_ACCEL, 10.), (0, 10.1),
-                            (CB.RES_ACCEL, 10.2), (0, 10.3)]
+                             (CB.RES_ACCEL, 10.), (0, 10.1),
+                             (CB.RES_ACCEL, 10.2), (0, 10.3)],
+    checks=[check_engaged],
   ),
   Maneuver(
     'while cruising at 60 mph, change cruise speed to 50mph',
     duration=30.,
     initial_speed=60. * CV.MPH_TO_MS,
     cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
-                            (CB.DECEL_SET, 10.), (0, 10.1),
-                            (CB.DECEL_SET, 10.2), (0, 10.3)]
+                             (CB.DECEL_SET, 10.), (0, 10.1),
+                             (CB.DECEL_SET, 10.2), (0, 10.3)],
+    checks=[check_engaged],
   ),
   Maneuver(
     'while cruising at 20mph, grade change +10%',
@@ -45,7 +57,8 @@ maneuvers = [
     initial_speed=20. * CV.MPH_TO_MS,
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
     grade_values = [0., 0., 1.0],
-    grade_breakpoints = [0., 10., 11.]
+    grade_breakpoints = [0., 10., 11.],
+    checks=[check_engaged],
   ),
   Maneuver(
     'while cruising at 20mph, grade change -10%',
@@ -53,7 +66,8 @@ maneuvers = [
     initial_speed=20. * CV.MPH_TO_MS,
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
     grade_values = [0., 0., -1.0],
-    grade_breakpoints = [0., 10., 11.]
+    grade_breakpoints = [0., 10., 11.],
+    checks=[check_engaged],
   ),
   Maneuver(
     'approaching a 40mph car while cruising at 60mph from 100m away',
@@ -63,7 +77,8 @@ maneuvers = [
     initial_distance_lead=100.,
     speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
     speed_lead_breakpoints = [0., 100.],
-    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     'approaching a 0mph car while cruising at 40mph from 150m away',
@@ -73,7 +88,8 @@ maneuvers = [
     initial_distance_lead=150.,
     speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
     speed_lead_breakpoints = [0., 100.],
-    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
@@ -83,7 +99,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values = [20., 20., 0.],
     speed_lead_breakpoints = [0., 15., 35.0],
-    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@@ -93,7 +110,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values = [20., 20., 0.],
     speed_lead_breakpoints = [0., 15., 25.0],
-    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@@ -103,7 +121,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values = [20., 20., 0.],
     speed_lead_breakpoints = [0., 15., 21.66],
-    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    checks=[check_engaged, check_fcw],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
@@ -113,7 +132,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values = [20., 20., 0.],
     speed_lead_breakpoints = [0., 15., 19.],
-    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+    cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+    checks=[check_engaged, check_fcw],
   ),
   Maneuver(
     'starting at 0mph, approaching a stopped car 100m away',
@@ -122,9 +142,10 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=100.,
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7),
-                            (CB.RES_ACCEL, 1.8), (0.0, 1.9)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
@@ -135,8 +156,9 @@ maneuvers = [
     speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
     speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
@@ -147,8 +169,9 @@ maneuvers = [
     speed_lead_values=[10., 0., 0., 10., 0.,10.],
     speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
@@ -159,11 +182,12 @@ maneuvers = [
     speed_lead_values=[0,  0 , 45],
     speed_lead_breakpoints=[0, 10., 40.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7),
-                            (CB.RES_ACCEL, 1.8), (0.0, 1.9),
-                            (CB.RES_ACCEL, 2.0), (0.0, 2.1),
-                            (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+                             (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "stop and go with 1m/s2 lead decel and accel, with full stops",
@@ -175,7 +199,8 @@ maneuvers = [
     speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
@@ -186,8 +211,9 @@ maneuvers = [
     speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
     speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
@@ -198,11 +224,12 @@ maneuvers = [
     speed_lead_values=[20., 10.],
     speed_lead_breakpoints=[1., 11.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7),
-                            (CB.RES_ACCEL, 1.8), (0.0, 1.9),
-                            (CB.RES_ACCEL, 2.0), (0.0, 2.1),
-                            (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+                             (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
@@ -213,11 +240,12 @@ maneuvers = [
     speed_lead_values=[20., 0.],
     speed_lead_breakpoints=[1., 11.],
     cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
-                            (CB.RES_ACCEL, 1.4), (0.0, 1.5),
-                            (CB.RES_ACCEL, 1.6), (0.0, 1.7),
-                            (CB.RES_ACCEL, 1.8), (0.0, 1.9),
-                            (CB.RES_ACCEL, 2.0), (0.0, 2.1),
-                            (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+                             (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+                             (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+                             (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+                             (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+                             (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+    checks=[check_engaged, check_no_collision],
   ),
   Maneuver(
     "fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
@@ -227,7 +255,8 @@ maneuvers = [
     initial_distance_lead=100.,
     speed_lead_values=[20.],
     speed_lead_breakpoints=[1.],
-    cruise_button_presses = []
+    cruise_button_presses = [],
+    checks=[check_fcw],
   ),
   Maneuver(
     "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
@@ -237,7 +266,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values=[20., 0.],
     speed_lead_breakpoints=[3., 23.],
-    cruise_button_presses = []
+    cruise_button_presses = [],
+    checks=[check_fcw],
   ),
   Maneuver(
     "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
@@ -247,7 +277,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values=[20., 0.],
     speed_lead_breakpoints=[3., 9.6],
-    cruise_button_presses = []
+    cruise_button_presses = [],
+    checks=[check_fcw],
   ),
   Maneuver(
     "fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
@@ -257,7 +288,8 @@ maneuvers = [
     initial_distance_lead=35.,
     speed_lead_values=[20., 0.],
     speed_lead_breakpoints=[3., 7.],
-    cruise_button_presses = []
+    cruise_button_presses = [],
+    checks=[check_fcw],
   )
 ]
 
@@ -293,6 +325,7 @@ def setup_output():
 class LongitudinalControl(unittest.TestCase):
   @classmethod
   def setUpClass(cls):
+    os.environ['NO_CAN_TIMEOUT'] = "1"
 
     setup_output()
 
@@ -314,26 +347,30 @@ class LongitudinalControl(unittest.TestCase):
   def test_longitudinal_setup(self):
     pass
 
-WORKERS = 8
 def run_maneuver_worker(k):
+  man = maneuvers[k]
   output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
-  for i, man in enumerate(maneuvers[k::WORKERS]):
+
+  def run(self):
+    print(man.title)
     manager.start_managed_process('radard')
     manager.start_managed_process('controlsd')
     manager.start_managed_process('plannerd')
 
-    score, plot = man.evaluate()
-    plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2))
+    plot, valid = man.evaluate()
+    plot.write_plot(output_dir, "maneuver" + str(k+1).zfill(2))
 
     manager.kill_managed_process('radard')
     manager.kill_managed_process('controlsd')
     manager.kill_managed_process('plannerd')
     time.sleep(5)
 
-for k in xrange(WORKERS):
-  setattr(LongitudinalControl,
-    "test_longitudinal_maneuvers_%d" % (k+1),
-    lambda self, k=k: run_maneuver_worker(k))
+    self.assertTrue(valid)
+
+  return run
+
+for k in range(len(maneuvers)):
+  setattr(LongitudinalControl, "test_longitudinal_maneuvers_%d" % (k+1), run_maneuver_worker(k))
 
 if __name__ == "__main__":
-  unittest.main()
+  unittest.main(failfast=True)

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
 params = Params()
@@ -142,8 +142,8 @@ def thermald_thread():
   ignition_seen = False
   started_seen = False
   thermal_status = ThermalStatus.green
-  health_sock.RCVTIMEO = 1500
-  current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
+  health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML)  # 2x the expected health frequency
+  current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
   health_prev = None
 
   # Make sure charging is enabled
@@ -191,13 +191,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:
@@ -268,7 +268,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

@@ -128,8 +128,7 @@ typedef struct UIScene {
   float v_cruise;
   uint64_t v_cruise_update_ts;
   float v_ego;
-  float v_curvature;
-  bool decel_for_turn;
+  bool decel_for_model;
 
   float speedlimit;
   bool speedlimit_valid;
@@ -267,13 +266,11 @@ typedef struct UIState {
   int is_metric_timeout;
   int longitudinal_control_timeout;
   int limit_set_speed_timeout;
-  int dragon_bbui_timeout;
 
   int status;
   bool is_metric;
   bool longitudinal_control;
   bool limit_set_speed;
-  bool dragon_bbui;
   float speed_lim_off;
   bool is_ego_over_limit;
   char alert_type[64];
@@ -296,6 +293,22 @@ typedef struct UIState {
   model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
 
   track_vertices_data track_vertices[2];
+
+  // dragonpilot
+  int dragon_ui_event_timeout;
+  int dragon_ui_maxspeed_timeout;
+  int dragon_ui_face_timeout;
+  int dragon_ui_dev_timeout;
+  int dragon_ui_dev_mini_timeout;
+  int dragon_enable_dashcam_timeout;
+
+  bool dragon_ui_event;
+  bool dragon_ui_maxspeed;
+  bool dragon_ui_face;
+  bool dragon_ui_dev;
+  bool dragon_ui_dev_mini;
+  bool dragon_enable_dashcam;
+
 } UIState;
 
 static int last_brightness = -1;
@@ -665,14 +678,27 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
   read_param_bool(&s->is_metric, "IsMetric");
   read_param_bool(&s->longitudinal_control, "LongitudinalControl");
   read_param_bool(&s->limit_set_speed, "LimitSetSpeed");
-  read_param_bool(&s->dragon_bbui, "DragonBBUI");
+  // dragonpilot
+  read_param_bool(&s->dragon_ui_event, "DragonUIEvent");
+  read_param_bool(&s->dragon_ui_maxspeed, "DragonUIMaxSpeed");
+  read_param_bool(&s->dragon_ui_face, "DragonUIFace");
+  read_param_bool(&s->dragon_ui_dev, "DragonUIDev");
+  read_param_bool(&s->dragon_ui_dev_mini, "DragonUIDevMini");
+  read_param_bool(&s->dragon_enable_dashcam, "DragonEnableDashcam");
 
 
   // Set offsets so params don't get read at the same time
   s->longitudinal_control_timeout = UI_FREQ / 3;
   s->is_metric_timeout = UI_FREQ / 2;
   s->limit_set_speed_timeout = UI_FREQ;
-  s->dragon_bbui_timeout = UI_FREQ / 4;
+
+  // dragonpilot, 1 sec
+  s->dragon_ui_event_timeout = 100;
+  s->dragon_ui_maxspeed_timeout = 100;
+  s->dragon_ui_face_timeout = 100;
+  s->dragon_ui_dev_timeout = 100;
+  s->dragon_ui_dev_mini_timeout = 100;
+  s->dragon_enable_dashcam_timeout = 100;
 }
 
 static void ui_draw_transformed_box(UIState *s, uint32_t color) {
@@ -1171,17 +1197,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) {
@@ -1312,7 +1327,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);
@@ -1424,13 +1439,17 @@ static void ui_draw_vision_header(UIState *s) {
   nvgRect(s->vg, ui_viz_rx, box_y, ui_viz_rw, header_h);
   nvgFill(s->vg);
 
-  //ui_draw_vision_maxspeed(s);
+  if (s->dragon_ui_maxspeed) {
+    ui_draw_vision_maxspeed(s);
+  }
 
 #ifdef SHOW_SPEEDLIMIT
   ui_draw_vision_speedlimit(s);
 #endif
   ui_draw_vision_speed(s);
-  //ui_draw_vision_event(s);
+  if (s->dragon_ui_event) {
+    ui_draw_vision_event(s);
+  }
 }
 
 static void ui_draw_infobar(UIState *s) {
@@ -1451,40 +1470,54 @@ static void ui_draw_infobar(UIState *s) {
   time_t t = time(NULL);
   struct tm tm = *localtime(&t);
 
-  char rel_steer[9];
-  snprintf(rel_steer, sizeof(rel_steer), "%s%05.1f°", s->scene.angleSteers < 0? "-" : "+", fabs(s->scene.angleSteers));
+  if (s->dragon_ui_dev_mini) {
+      char rel_steer[9];
+      snprintf(rel_steer, sizeof(rel_steer), "%s%05.1f°", s->scene.angleSteers < 0? "-" : "+", fabs(s->scene.angleSteers));
 
-  char des_steer[9];
-  if (s->scene.engaged) {
-    snprintf(des_steer, sizeof(des_steer), "%s%05.1f°", s->scene.angleSteersDes < 0? "-" : "+", fabs(s->scene.angleSteersDes));
+      char des_steer[9];
+      if (s->scene.engaged) {
+        snprintf(des_steer, sizeof(des_steer), "%s%05.1f°", s->scene.angleSteersDes < 0? "-" : "+", fabs(s->scene.angleSteersDes));
+      } else {
+        snprintf(des_steer, sizeof(des_steer), "%7s", "N/A");
+      }
+
+
+      char lead_dist[8];
+      if (s->scene.lead_status) {
+        snprintf(lead_dist, sizeof(lead_dist), "%06.2fm", s->scene.lead_d_rel);
+      } else {
+        snprintf(lead_dist, sizeof(lead_dist), "%7s", "N/A");
+      }
+
+
+      snprintf(
+        infobar,
+        sizeof(infobar),
+        "%04d/%02d/%02d %02d:%02d:%02d | REL: %s | DES: %s | DIST: %s",
+        tm.tm_year + 1900,
+        tm.tm_mon + 1,
+        tm.tm_mday,
+        tm.tm_hour,
+        tm.tm_min,
+        tm.tm_sec,
+        rel_steer,
+        des_steer,
+        lead_dist
+      );
   } else {
-    snprintf(des_steer, sizeof(des_steer), "%7s", "N/A");
+      snprintf(
+        infobar,
+        sizeof(infobar),
+        "%04d/%02d/%02d %02d:%02d:%02d",
+        tm.tm_year + 1900,
+        tm.tm_mon + 1,
+        tm.tm_mday,
+        tm.tm_hour,
+        tm.tm_min,
+        tm.tm_sec
+      );
   }
 
-
-  char lead_dist[8];
-  if (s->scene.lead_status) {
-    snprintf(lead_dist, sizeof(lead_dist), "%06.2fm", s->scene.lead_d_rel);
-  } else {
-    snprintf(lead_dist, sizeof(lead_dist), "%7s", "N/A");
-  }
-
-
-  snprintf(
-    infobar,
-    sizeof(infobar),
-    "%04d/%02d/%02d %02d:%02d:%02d | REL: %s | DES: %s | DIST: %s",
-    tm.tm_year + 1900,
-    tm.tm_mon + 1,
-    tm.tm_mday,
-    tm.tm_hour,
-    tm.tm_min,
-    tm.tm_sec,
-    rel_steer,
-    des_steer,
-    lead_dist
-  );
-
   nvgBeginPath(s->vg);
   nvgRoundedRect(s->vg, rect_x + sidebar_offset, rect_y, rect_w, rect_h, 15);
   nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100));
@@ -1707,15 +1740,19 @@ static void ui_draw_vision_footer(UIState *s) {
   nvgBeginPath(s->vg);
   nvgRect(s->vg, ui_viz_rx, footer_y, ui_viz_rw, footer_h);
 
-  //ui_draw_vision_face(s);
+  if (s->dragon_ui_face) {
+    ui_draw_vision_face(s);
+  }
 
 #ifdef SHOW_SPEEDLIMIT
   ui_draw_vision_map(s);
 #endif
-  if (s->dragon_bbui) {
+  if (s->dragon_ui_dev) {
     ui_draw_bbui(s);
   }
-  ui_draw_infobar(s);
+  if (s->dragon_ui_dev_mini || s->dragon_enable_dashcam) {
+    ui_draw_infobar(s);
+  }
 }
 
 static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
@@ -1931,8 +1968,7 @@ void handle_message(UIState *s, void *which) {
 
     s->scene.frontview = datad.rearViewCam;
 
-    s->scene.v_curvature = datad.vCurvature;
-    s->scene.decel_for_turn = datad.decelForTurn;
+    s->scene.decel_for_model = datad.decelForModel;
 
     s->scene.angleSteers = datad.angleSteers;
     s->scene.angleSteersDes = datad.angleSteersDes;
@@ -2616,7 +2652,13 @@ int main(int argc, char* argv[]) {
     read_param_bool_timeout(&s->longitudinal_control, "LongitudinalControl", &s->longitudinal_control_timeout);
     read_param_bool_timeout(&s->limit_set_speed, "LimitSetSpeed", &s->limit_set_speed_timeout);
     read_param_float_timeout(&s->speed_lim_off, "SpeedLimitOffset", &s->limit_set_speed_timeout);
-    read_param_bool_timeout(&s->dragon_bbui, "DragonBBUI", &s->dragon_bbui_timeout);
+    // dragonpilot
+    read_param_bool_timeout(&s->dragon_ui_event, "DragonUIEvent", &s->dragon_ui_event_timeout);
+    read_param_bool_timeout(&s->dragon_ui_maxspeed, "DragonUIMaxSpeed", &s->dragon_ui_maxspeed_timeout);
+    read_param_bool_timeout(&s->dragon_ui_face, "DragonUIFace", &s->dragon_ui_face_timeout);
+    read_param_bool_timeout(&s->dragon_ui_dev, "DragonUIDev", &s->dragon_ui_dev_timeout);
+    read_param_bool_timeout(&s->dragon_ui_dev_mini, "DragonUIDevMini", &s->dragon_ui_dev_mini_timeout);
+    read_param_bool_timeout(&s->dragon_enable_dashcam, "DragonEnableDashcam", &s->dragon_enable_dashcam_timeout);
 
     pthread_mutex_unlock(&s->lock);
 

selfdrive/visiond/models/commonmodel.c

@@ -97,39 +97,70 @@ static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, con
   for (int i=0; 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;
+  }
+}
 
-  cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
-  struct cereal_ModelData_PathData d = {
+
+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.dist,
     .prob = data.prob,
     .std = data.std,
-    .poly = poly_ptr,
+    .relY = data.rel_y,
+    .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_PathData(&d, ret);
-  return ret;
+  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;
 
-  cereal_ModelData_LeadData_ptr leadp = cereal_new_ModelData_LeadData(cs);
-  struct cereal_ModelData_LeadData leadd = (struct cereal_ModelData_LeadData){
-    .dist = data.lead.dist,
-    .prob = data.lead.prob,
-    .std = data.lead.std,
-    .relVel = data.lead.rel_v,
-    .relVelStd = data.lead.rel_v_std,
-  };
-  cereal_write_ModelData_LeadData(&leadd, leadp);
-
 
   capn_list32 input_transform_ptr = capn_new_list32(cs, 3*3);
   for (int i = 0; i < 3 * 3; i++) {
     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
-
+#define LEAD_MDN_N 5 // probs for 5 groups
+#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.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
-  model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
+  model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
+  model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
+  model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
 
   model.path.prob = 1.;
   model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
@@ -105,17 +111,59 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
 
   const double max_dist = 140.0;
   const double max_rel_vel = 10.0;
+  // current lead
   int mdn_max_idx = 0;
   for (int i=1; i<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*5]);
-  model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist;
-  model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist;
-  model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel;
-  model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel;
+  model.lead.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_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
+  model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
+  model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
+  model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
+  model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
+  model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
+  model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
+  model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
+  model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
+
+
+  // get speed percentiles numbers represent 5th, 15th, ... 95th percentile
+  for (int i=0; i < SPEED_PERCENTILES; i++) {
+    model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
+  }
+  float sum = 0;
+  for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
+    sum += net_outputs.speed[idx];
+    int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
+    if (idx_percentile < SPEED_PERCENTILES ){
+      model.speed[idx_percentile] = ((float)idx)/2.0;
+    }
+  }
+  // make sure no percentiles are skipped
+  for (int i=SPEED_PERCENTILES-1; i > 0; i--){
+    if (model.speed[i-1] > model.speed[i]){
+      model.speed[i-1] = model.speed[i];
+    }
+  }
   return model;
 }
 

selfdrive/visiond/models/monitoring.cc

@@ -37,9 +37,11 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
   s->m->execute(net_input_buf);
 
   MonitoringResult ret = {0};
-  memcpy(ret.vs, s->output, sizeof(ret.vs));
-  ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1];
-
+  memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
+  memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position);
+  memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
+  memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
+  memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
   return ret;
 }
 

selfdrive/visiond/models/monitoring.h

@@ -8,11 +8,18 @@
 extern "C" {
 #endif
 
-#define OUTPUT_SIZE 8
+#define OUTPUT_SIZE_DEPRECATED 8
+#define OUTPUT_SIZE 31
 
 typedef struct MonitoringResult {
-  float vs[OUTPUT_SIZE - 1];
-  float std;
+  float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
+  float std_DEPRECATED;
+
+  float face_orientation[3];
+  float face_position[2];
+  float face_prob;
+  float left_eye_prob;
+  float right_eye_prob;
 } MonitoringResult;
 
 typedef struct MonitoringState {

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));
-        framed.setDescriptor(descriptor_vs);
+        // junk 0s from legacy model
+        //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;