diff --git a/Pipfile b/Pipfile
index 6b9f7e95b..7afffb1e9 100644
--- a/Pipfile
+++ b/Pipfile
@@ -99,6 +99,8 @@ simplejson = "*"
python-logstash-async = "*"
pandas = "*"
seaborn = "*"
+tensorflow-estimator = "==1.10.12"
+pyproj = "*"
[packages]
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
diff --git a/Pipfile.lock b/Pipfile.lock
index 30c43e286..15416a7bf 100644
--- a/Pipfile.lock
+++ b/Pipfile.lock
@@ -1,7 +1,7 @@
{
"_meta": {
"hash": {
- "sha256": "89070d7d9478ac9e6ad2c9848aaf724cb7362a1de4af9f8fb8c40be5f37c043d"
+ "sha256": "43f96719ff9a1b7eae6c7a88f903c4f5a137c2b195e3ba148f32767827b98710"
},
"pipfile-spec": 6,
"requires": {
@@ -656,12 +656,12 @@
},
"aenum": {
"hashes": [
- "sha256:058f0cfaf911899dc21b334362047df74ce989335dd8dff8e4be1a6313b15232",
- "sha256:6af970173d9b4ac0384ad7d1cfe9523eeb9a3578793e1664090c13cb59df6469",
- "sha256:80f14366578d84f6bccb0670259744cb3a7f2ab504480c306238a23cdd569457"
+ "sha256:0e3589654ef090784971f7778dcb74b08c9b4ef80b33267c00f82ddeedac179a",
+ "sha256:b12a7be3d89b270f266f8643aaa126404e5cdc0929bd6f09548b8eaed85e2aa1",
+ "sha256:e4dab068cbe00295bbc3660cd562221b008687d0f7a4c40fc2dd7a80002126a7"
],
"index": "pypi",
- "version": "==2.2.0"
+ "version": "==2.2.1"
},
"amqp": {
"hashes": [
@@ -817,18 +817,18 @@
},
"boto3": {
"hashes": [
- "sha256:34a8ddb7247316be6ea94c7eeee41212312d250d99bf668fcd6748629b578622",
- "sha256:71f3554cc69fa20be06cf20d6c9e0d8095d7c40695b48618676c3cd9a5ba0783"
+ "sha256:6a950bf98b22812896ea0f833a26d448acfdf43179f41f389d501af7a9fae328",
+ "sha256:cfbc062a76a7781af8e6a4ea26ebcafa3866872a8cceb05fdbf588c36e7848f0"
],
"index": "pypi",
- "version": "==1.9.189"
+ "version": "==1.9.195"
},
"botocore": {
"hashes": [
- "sha256:4febbf206d1dc8b8299aa211d8e382d5bf3f22097855b9f98d5e8c401ef8192b",
- "sha256:b62ab3e4e98e075fc9e8e8fd4e8f5b92ebf311a6dc9f7578650938c7bc94e592"
+ "sha256:691627c2aeff0fcbd9237985717c28404a628181fd3e86b7be500bf2ee156007",
+ "sha256:c59e9981db9dfc54f0d22f731ca8de904760049a9c60d86dcedde84ae64ef4f0"
],
- "version": "==1.12.189"
+ "version": "==1.12.195"
},
"celery": {
"hashes": [
@@ -1734,7 +1734,7 @@
"sha256:23953d55076df038541e648a53676fb24980f7a1be290cdda21300b3bc21dfb0",
"sha256:552a91f381532e33cbd07c6a2655a21908088962bb8fa7239ecbcc6ad1140cc7"
],
- "markers": "python_version == '2.7'",
+ "markers": "python_version == '2.6' or python_version == '2.7' or python_version == '3.0' or python_version == '3.1' or python_version == '3.2'",
"version": "==1.5"
},
"mpld3": {
@@ -2264,6 +2264,32 @@
"index": "pypi",
"version": "==0.1.17"
},
+ "pyproj": {
+ "hashes": [
+ "sha256:1303b343a1c4d83d1ac17494571d958335c0a4597d4aa623cd64df1f36fb18bf",
+ "sha256:1fd2f756f43ba762f5a1ae769c6cc9ae6f919996bf80c621f331e4a5d89ae74f",
+ "sha256:2b3f59b16cde23c0d95b1d9ba3dd1c0be3937d877df3b47a522d3c9a74d31668",
+ "sha256:2e5d8b892782db22c3d6ffcb272c8ac0a18995e22d45db762a893ec64cc039d6",
+ "sha256:512a59bc20587f88df78a69872ed8cb4ec7905415ba85b4de0af35601770fba1",
+ "sha256:5aa89b04855e439e2b96caf1f49fa2ab60e6e7c407034284d31d808abed0618a",
+ "sha256:75b4c5eba36060a00867e16e81755a44e93a95b0bbaa4d6ee40f10c330b62108",
+ "sha256:860fde3429f364767c88a8e7f14f330e3fdace9f438e0ef1b112108a44c1eee8",
+ "sha256:86886f21af819553990f0da9ad7737d52cd32076dd277db34e40cf805919e839",
+ "sha256:9417d46f2b5e6ba79bd6adea0aae9659501c8a0350a5a98c0d9cb1442d45b0b4",
+ "sha256:9bb81141715832bb8682dda3c2f8f1ad0f43cf8aec5db52e0992c16c7664e227",
+ "sha256:afe150d8f0b351df7a357cfce50a8a5c0a44bdfeec5e08075f873cfeddf63004",
+ "sha256:cc2e21c09245c3eb47014c3d4c58888cc8d48a168a03533f9ed3f8b2ee82a352",
+ "sha256:cefb4a1a5d51cd04c9dd31bb18047c2dafcb86bc33a0347746b5a0ef82c071d8",
+ "sha256:cfa2cd0ee23bcfc9f165b4f8e9c5b7ba829c7b49175fb73420544619148d91af",
+ "sha256:d3a78ce50e6c50e701d65929dc04ab751edaeae660455a9fef3c2231d609f8c1",
+ "sha256:d772ee53c8d316de1b7f68244ab654f864553bca177357f8787a65dc09816ae3",
+ "sha256:ddd99d6f2c813e44721c22039ddf333e4b95e8d0794b03d9962c94c01c823934",
+ "sha256:ec89e68a0cf210af0cc2724b5f8601d4b6809ff0f556e16efc8c955e79672f7a",
+ "sha256:f118762ed0abe8e988a20020241fe1e6133edb960d2665a44d6f48acc6d8e7c7"
+ ],
+ "index": "pypi",
+ "version": "==2.2.1"
+ },
"pyrsistent": {
"hashes": [
"sha256:50cffebc87ca91b9d4be2dcc2e479272bcb466b5a0487b6c271f7ddea6917e14"
@@ -2424,10 +2450,10 @@
},
"qtconsole": {
"hashes": [
- "sha256:4af84facdd6f00a6b9b2927255f717bb23ae4b7a20ba1d9ef0a5a5a8dbe01ae2",
- "sha256:60d61d93f7d67ba2b265c6d599d413ffec21202fec999a952f658ff3a73d252b"
+ "sha256:6a85456af7a98b0f554d140922b7b6a219757b039adb2b95e847cf115eaa20ae",
+ "sha256:767eb9ec3f9943bc84270198b5ff95d2d86d68d6b57792fafa4df4fc6b16cd7c"
],
- "version": "==4.5.1"
+ "version": "==4.5.2"
},
"redis": {
"hashes": [
@@ -2685,9 +2711,10 @@
},
"tensorflow-estimator": {
"hashes": [
- "sha256:ca073f66063407a091d610ec1b22e39ea30248710198cc6f13769320bdbe3992"
+ "sha256:3e460f43682c7d789e5fe966630029558434d32502e632ee7f6703451258528c"
],
- "version": "==1.14.0"
+ "index": "pypi",
+ "version": "==1.10.12"
},
"tensorflow-gpu": {
"hashes": [
diff --git a/README.md b/README.md
index f6e3fbe79..a4e47fc44 100644
--- a/README.md
+++ b/README.md
@@ -71,7 +71,7 @@ Supported Cars
| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
| GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| 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 |
diff --git a/RELEASES.md b/RELEASES.md
index 07bbc6208..f28977e92 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,15 @@
+Version 0.6.2 (2019-07-29)
+========================
+ * New driving model!
+ * Improve lane tracking with double lines
+ * Strongly improve stationary vehicle detection
+ * Strongly reduce cases of braking due to false leads
+ * Better lead tracking around turns
+ * Improve cut-in prediction by using neural network
+ * Improve lateral control on Toyota Camry and C-HR thanks to zorrobyte!
+ * Fix unintended openpilot disengagements on Jeep thanks to adhintz!
+ * Fix delayed transition to offroad when car is turned off
+
Version 0.6.1 (2019-07-21)
========================
* Remote SSH with comma prime and [ssh.comma.ai](https://ssh.comma.ai)
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index 000a4ebc6..c051e86c7 100644
Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 8797380c9..21d818265 100644
--- a/cereal/car.capnp
+++ b/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
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 3dbbc516d..5f76cbd74 100644
--- a/cereal/log.capnp
+++ b/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 {
diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py
deleted file mode 100644
index fbed218f7..000000000
--- a/common/kalman/ekf.py
+++ /dev/null
@@ -1,253 +0,0 @@
-# pylint: skip-file
-from __future__ import print_function
-import abc
-import numpy as np
-# The EKF class contains the framework for an Extended Kalman Filter, but must be subclassed to use.
-# A subclass must implement:
-# 1) calc_transfer_fun(); see bottom of file for more info.
-# 2) __init__() to initialize self.state, self.covar, and self.process_noise appropriately
-
-# Alternatively, the existing implementations of EKF can be used (e.g. EKF2D)
-
-# Sensor classes are optionally used to pass measurement information into the EKF, to keep
-# sensor parameters and processing methods for a each sensor together.
-# Sensor classes have a read() method which takes raw sensor data and returns
-# a SensorReading object, which can be passed to the EKF update() method.
-
-# For usage, see run_ekf1d.py in selfdrive/new for a simple example.
-# ekf.predict(dt) should be called between update cycles with the time since it was last called.
-# Ideally, predict(dt) should be called at a relatively constant rate.
-# update() should be called once per sensor, and can be called multiple times between predict steps.
-# Access and set the state of the filter directly with ekf.state and ekf.covar.
-
-
-class SensorReading:
- # Given a perfect model and no noise, data = obs_model * state
- def __init__(self, data, covar, obs_model):
- self.data = data
- self.obs_model = obs_model
- self.covar = covar
-
- def __repr__(self):
- return "SensorReading(data={}, covar={}, obs_model={})".format(
- repr(self.data), repr(self.covar), repr(self.obs_model))
-
-
-# A generic sensor class that does no pre-processing of data
-class SimpleSensor:
- # obs_model can be
- # a full observation model matrix, or
- # an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
- # covar can be
- # a full covariance matrix
- # a float or tuple of individual covars for each component of the sensor reading
- # dims is the number of states in the EKF
- def __init__(self, obs_model, covar, dims):
- # Allow for integer covar/obs_model
- if not hasattr(obs_model, "__len__"):
- obs_model = (obs_model, )
- if not hasattr(covar, "__len__"):
- covar = (covar, )
-
- # Full observation model passed
- if dims in np.array(obs_model).shape:
- self.obs_model = np.asmatrix(obs_model)
- self.covar = np.asmatrix(covar)
- # Indices of unit observations passed
- else:
- self.obs_model = np.matlib.zeros((len(obs_model), dims))
- self.obs_model[:, list(obs_model)] = np.identity(len(obs_model))
- if np.asarray(covar).ndim == 2:
- self.covar = np.asmatrix(covar)
- elif len(covar) == len(obs_model):
- self.covar = np.matlib.diag(covar)
- else:
- self.covar = np.matlib.identity(len(obs_model)) * covar
-
- def read(self, data, covar=None):
- if covar:
- self.covar = covar
- return SensorReading(data, self.covar, self.obs_model)
-
-
-class EKF:
- __metaclass__ = abc.ABCMeta
-
- def __init__(self, debug=False):
- self.DEBUG = debug
-
-
- def __str__(self):
- return "EKF(state={}, covar={})".format(self.state, self.covar)
-
- # Measurement update
- # Reading should be a SensorReading object with data, covar, and obs_model attributes
- def update(self, reading):
- # Potential improvements:
- # deal with negative covars
- # add noise to really low covars to ensure stability
- # use mahalanobis distance to reject outliers
- # wrap angles after state updates and innovation
-
- # y = z - H*x
- innovation = reading.data - reading.obs_model * self.state
-
- if self.DEBUG:
- print("reading:\n",reading.data)
- print("innovation:\n",innovation)
-
- # S = H*P*H' + R
- innovation_covar = reading.obs_model * self.covar * reading.obs_model.T + reading.covar
-
- # K = P*H'*S^-1
- kalman_gain = self.covar * reading.obs_model.T * np.linalg.inv(
- innovation_covar)
-
- if self.DEBUG:
- print("gain:\n", kalman_gain)
- print("innovation_covar:\n", innovation_covar)
- print("innovation: ", innovation)
- print("test: ", self.covar * reading.obs_model.T * (
- reading.obs_model * self.covar * reading.obs_model.T + reading.covar *
- 0).I)
-
- # x = x + K*y
- self.state += kalman_gain*innovation
-
- # print "covar", np.diag(self.covar)
- #self.state[(roll_vel, yaw_vel, pitch_vel),:] = reading.data
-
- # Standard form: P = (I - K*H)*P
- # self.covar = (self.identity - kalman_gain*reading.obs_model) * self.covar
-
- # Use the Joseph form for numerical stability: P = (I-K*H)*P*(I - K*H)' + K*R*K'
- aux_mtrx = (self.identity - kalman_gain * reading.obs_model)
- self.covar = aux_mtrx * self.covar * aux_mtrx.T + kalman_gain * reading.covar * kalman_gain.T
-
- if self.DEBUG:
- print("After update")
- print("state\n", self.state)
- print("covar:\n",self.covar)
-
- def update_scalar(self, reading):
- # like update but knowing that measurement is a scalar
- # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
-
- # innovation = reading.data - np.matmul(reading.obs_model, self.state)
- # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
- # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
- # self.state += np.matmul(kalman_gain, innovation)
- # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
- # self.covar = np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
-
- # written without np.matmul
- es = np.einsum
- ABC_T = "ij,jk,lk->il"
- AB_T = "ij,kj->ik"
- AB = "ij,jk->ik"
- innovation = reading.data - es(AB, reading.obs_model, self.state)
- innovation_covar = es(ABC_T, reading.obs_model, self.covar,
- reading.obs_model) + reading.covar
- kalman_gain = es(AB_T, self.covar, reading.obs_model) / innovation_covar
-
- self.state += es(AB, kalman_gain, innovation)
- aux_mtrx = self.identity - es(AB, kalman_gain, reading.obs_model)
- self.covar = es(ABC_T, aux_mtrx, self.covar, aux_mtrx) + \
- es(ABC_T, kalman_gain, reading.covar, kalman_gain)
-
- # Prediction update
- def predict(self, dt):
- es = np.einsum
- ABC_T = "ij,jk,lk->il"
- AB = "ij,jk->ik"
-
- # State update
- transfer_fun, transfer_fun_jacobian = self.calc_transfer_fun(dt)
-
- # self.state = np.matmul(transfer_fun, self.state)
- # self.covar = np.matmul(np.matmul(transfer_fun_jacobian, self.covar), transfer_fun_jacobian.T) + self.process_noise * dt
-
- # x = f(x, u), written in the form x = A(x, u)*x
- self.state = es(AB, transfer_fun, self.state)
-
- # P = J*P*J' + Q
- self.covar = es(ABC_T, transfer_fun_jacobian, self.covar,
- transfer_fun_jacobian) + self.process_noise * dt #!dt
-
- #! Clip covariance to avoid explosions
- self.covar = np.clip(self.covar,-1e10,1e10)
-
- @abc.abstractmethod
- def calc_transfer_fun(self, dt):
- """Return a tuple with the transfer function and transfer function jacobian
- The transfer function and jacobian should both be a numpy matrix of size DIMSxDIMS
-
- The transfer function matrix A should satisfy the state-update equation
- x_(k+1) = A * x_k
-
- The jacobian J is the direct jacobian A*x_k. For linear systems J=A.
-
- Current implementations calculate A and J as functions of state. Control input
- can be added trivially by adding a control parameter to predict() and calc_tranfer_update(),
- and using it during calculation of A and J
- """
-
-
-class FastEKF1D(EKF):
- """Fast version of EKF for 1D problems with scalar readings."""
-
- def __init__(self, dt, var_init, Q):
- super(FastEKF1D, self).__init__(False)
- self.state = [0, 0]
- self.covar = [var_init, var_init, 0]
-
- # Process Noise
- self.dtQ0 = dt * Q[0]
- self.dtQ1 = dt * Q[1]
-
- def update(self, reading):
- raise NotImplementedError
-
- def update_scalar(self, reading):
- # TODO(mgraczyk): Delete this for speed.
- # assert np.all(reading.obs_model == [1, 0])
-
- rcov = reading.covar[0, 0]
-
- x = self.state
- S = self.covar
-
- innovation = reading.data - x[0]
- innovation_covar = S[0] + rcov
-
- k0 = S[0] / innovation_covar
- k1 = S[2] / innovation_covar
-
- x[0] += k0 * innovation
- x[1] += k1 * innovation
-
- mk = 1 - k0
- S[1] += k1 * (k1 * (S[0] + rcov) - 2 * S[2])
- S[2] = mk * (S[2] - k1 * S[0]) + rcov * k0 * k1
- S[0] = mk * mk * S[0] + rcov * k0 * k0
-
- def predict(self, dt):
- # State update
- x = self.state
-
- x[0] += dt * x[1]
-
- # P = J*P*J' + Q
- S = self.covar
- S[0] += dt * (2 * S[2] + dt * S[1]) + self.dtQ0
- S[2] += dt * S[1]
- S[1] += self.dtQ1
-
- # Clip covariance to avoid explosions
- S = max(-1e10, min(S, 1e10))
-
- def calc_transfer_fun(self, dt):
- tf = np.identity(2)
- tf[0, 1] = dt
- tfj = tf
- return tf, tfj
diff --git a/common/kalman/tests/test_ekf.py b/common/kalman/tests/test_ekf.py
deleted file mode 100644
index 3d6da323d..000000000
--- a/common/kalman/tests/test_ekf.py
+++ /dev/null
@@ -1,116 +0,0 @@
-import numpy as np
-import numpy.matlib
-import unittest
-import timeit
-
-from common.kalman.ekf import EKF, SimpleSensor, FastEKF1D
-
-class TestEKF(EKF):
- def __init__(self, var_init, Q):
- super(TestEKF, self).__init__(False)
- self.identity = numpy.matlib.identity(2)
- self.state = numpy.matlib.zeros((2, 1))
- self.covar = self.identity * var_init
-
- self.process_noise = numpy.matlib.diag(Q)
-
- def calc_transfer_fun(self, dt):
- tf = numpy.matlib.identity(2)
- tf[0, 1] = dt
- return tf, tf
-
-
-class EKFTest(unittest.TestCase):
- def test_update_scalar(self):
- ekf = TestEKF(1e3, [0.1, 1])
- dt = 1. / 100
-
- sensor = SimpleSensor(0, 1, 2)
- readings = map(sensor.read, np.arange(100, 300))
-
- for reading in readings:
- ekf.update_scalar(reading)
- ekf.predict(dt)
-
- np.testing.assert_allclose(ekf.state, [[300], [100]], 1e-4)
- np.testing.assert_allclose(
- ekf.covar,
- np.asarray([[0.0563, 0.10278], [0.10278, 0.55779]]),
- atol=1e-4)
-
- def test_unbiased(self):
- ekf = TestEKF(1e3, [0., 0.])
- dt = np.float64(1. / 100)
-
- sensor = SimpleSensor(0, 1, 2)
- readings = map(sensor.read, np.arange(1000))
-
- for reading in readings:
- ekf.update_scalar(reading)
- ekf.predict(dt)
-
- np.testing.assert_allclose(ekf.state, [[1000.], [100.]], 1e-4)
-
-
-class FastEKF1DTest(unittest.TestCase):
- def test_correctness(self):
- dt = 1. / 100
- reading = SimpleSensor(0, 1, 2).read(100)
-
- ekf = TestEKF(1e3, [0.1, 1])
- fast_ekf = FastEKF1D(dt, 1e3, [0.1, 1])
-
- ekf.update_scalar(reading)
- fast_ekf.update_scalar(reading)
- self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0])
- self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1])
- self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0])
- self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2])
- self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1])
-
- ekf.predict(dt)
- fast_ekf.predict(dt)
- self.assertAlmostEqual(ekf.state[0] , fast_ekf.state[0])
- self.assertAlmostEqual(ekf.state[1] , fast_ekf.state[1])
- self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0])
- self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2])
- self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1])
-
- def test_speed(self):
- setup = """
-import numpy as np
-from common.kalman.tests.test_ekf import TestEKF
-from common.kalman.ekf import SimpleSensor, FastEKF1D
-
-dt = 1. / 100
-reading = SimpleSensor(0, 1, 2).read(100)
-
-var_init, Q = 1e3, [0.1, 1]
-ekf = TestEKF(var_init, Q)
-fast_ekf = FastEKF1D(dt, var_init, Q)
- """
-
- timeit.timeit("""
-ekf.update_scalar(reading)
-ekf.predict(dt)
- """, setup=setup, number=1000)
-
- ekf_speed = timeit.timeit("""
-ekf.update_scalar(reading)
-ekf.predict(dt)
- """, setup=setup, number=20000)
-
- timeit.timeit("""
-fast_ekf.update_scalar(reading)
-fast_ekf.predict(dt)
- """, setup=setup, number=1000)
-
- fast_ekf_speed = timeit.timeit("""
-fast_ekf.update_scalar(reading)
-fast_ekf.predict(dt)
- """, setup=setup, number=20000)
-
- assert fast_ekf_speed < ekf_speed / 4
-
-if __name__ == "__main__":
- unittest.main()
diff --git a/common/params.py b/common/params.py
index 4a2bf44a3..ee8d18794 100755
--- a/common/params.py
+++ b/common/params.py
@@ -53,6 +53,7 @@ keys = {
"AthenadPid": [TxType.PERSISTENT],
"CalibrationParams": [TxType.PERSISTENT],
"CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
+ "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"CompletedTrainingVersion": [TxType.PERSISTENT],
"ControlsParams": [TxType.PERSISTENT],
"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
@@ -70,6 +71,7 @@ keys = {
"IsUploadRawEnabled": [TxType.PERSISTENT],
"IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT],
"LimitSetSpeed": [TxType.PERSISTENT],
+ "LimitSetSpeedNeural": [TxType.PERSISTENT],
"LiveParameters": [TxType.PERSISTENT],
"LongitudinalControl": [TxType.PERSISTENT],
"Passive": [TxType.PERSISTENT],
@@ -82,16 +84,29 @@ keys = {
"Version": [TxType.PERSISTENT],
#dragonpilot config
"DragonEnableDashcam": [TxType.PERSISTENT],
- "DragonDisableDriverSafetyCheck": [TxType.PERSISTENT],
+ "DragonDisableDriverSafetyCheck": [TxType.PERSISTENT], # deprecated
+ "DragonEnableDriverSafetyCheck": [TxType.PERSISTENT],
"DragonAutoShutdownAt": [TxType.PERSISTENT],
- "DragonTempDisableSteerOnSignal": [TxType.PERSISTENT],
- "DragonDisableLogger": [TxType.PERSISTENT],
- "DragonDisableUploader": [TxType.PERSISTENT],
+ "DragonTempDisableSteerOnSignal": [TxType.PERSISTENT], # deprecated
+ "DragonEnableSteeringOnSignal": [TxType.PERSISTENT],
+ "DragonDisableLogger": [TxType.PERSISTENT], # deprecated
+ "DragonEnableLogger": [TxType.PERSISTENT],
+ "DragonDisableUploader": [TxType.PERSISTENT], # deprecated
+ "DragonEnableUploader": [TxType.PERSISTENT],
"DragonNoctuaMode": [TxType.PERSISTENT],
"DragonCacheCar": [TxType.PERSISTENT],
"DragonCachedModel": [TxType.PERSISTENT],
"DragonCachedFP": [TxType.PERSISTENT],
"DragonCachedVIN": [TxType.PERSISTENT],
+ "DragonAllowGas": [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],
}
diff --git a/common/realtime.py b/common/realtime.py
index 8bcd06966..6fe64d0d7 100644
--- a/common/realtime.py
+++ b/common/realtime.py
@@ -20,6 +20,7 @@ DT_CTRL = 0.01 # controlsd
DT_PLAN = 0.05 # mpc
DT_MDL = 0.05 # model
DT_DMON = 0.1 # driver monitoring
+DT_TRML = 0.5 # thermald and manager
ffi = FFI()
diff --git a/common/vin.py b/common/vin.py
index 29357e34b..989aa0fa1 100755
--- a/common/vin.py
+++ b/common/vin.py
@@ -1,61 +1,8 @@
#!/usr/bin/env python
-import time
-from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.boardd.boardd import can_list_to_can_capnp
-def get_vin(logcan, sendcan):
-
- # works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
- # Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
- query_msg = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), 0],
- [0x7e0, 0, '\x30'.ljust(8, "\x00"), 0]]
-
- cnts = [1, 2] # Number of messages to wait for at each iteration
- vin_valid = True
-
- dat = []
- for i in range(len(query_msg)):
- cnt = 0
- sendcan.send(can_list_to_can_capnp([query_msg[i]], msgtype='sendcan'))
- got_response = False
- t_start = sec_since_boot()
- while sec_since_boot() - t_start < 0.05 and not got_response:
- for a in messaging.drain_sock(logcan):
- for can in a.can:
- if can.src == 0 and can.address == 0x7e8:
- vin_valid = vin_valid and is_vin_response_valid(can.dat, i, cnt)
- dat += can.dat[2:] if i == 0 else can.dat[1:]
- cnt += 1
- if cnt == cnts[i]:
- got_response = True
- time.sleep(0.01)
-
- return "".join(dat[3:]) if vin_valid else ""
-
-"""
-if 'vin' not in gctx:
- print "getting vin"
- gctx['vin'] = query_vin()[3:]
- print "got VIN %s" % (gctx['vin'],)
- cloudlog.info("got VIN %s" % (gctx['vin'],))
-
-# *** determine platform based on VIN ****
-if vin.startswith("19UDE2F36G"):
- print "ACURA ILX 2016"
- self.civic = False
-else:
- # TODO: add Honda check explicitly
- print "HONDA CIVIC 2016"
- self.civic = True
-
-# *** special case VIN of Acura test platform
-if vin == "19UDE2F36GA001322":
- print "comma.ai test platform detected"
- # it has a gas interceptor and a torque mod
- self.torque_mod = True
-"""
-
+VIN_UNKNOWN = "0" * 17
# sanity checks on response messages from vin query
def is_vin_response_valid(can_dat, step, cnt):
@@ -84,12 +31,71 @@ def is_vin_response_valid(can_dat, step, cnt):
return True
-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)
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index 287c72a78..0a10c59e5 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -12,18 +12,18 @@ fi
function launch {
# apply update
- if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
- git reset --hard @{u} &&
- git clean -xdf &&
-
- # Touch all files on release2 after checkout to prevent rebuild
- BRANCH=$(git rev-parse --abbrev-ref HEAD)
- if [[ "$BRANCH" == "release2" ]]; then
- touch **
- fi
-
- exec "${BASH_SOURCE[0]}"
- fi
+# if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
+# git reset --hard @{u} &&
+# git clean -xdf &&
+#
+# # Touch all files on release2 after checkout to prevent rebuild
+# BRANCH=$(git rev-parse --abbrev-ref HEAD)
+# if [[ "$BRANCH" == "release2" ]]; then
+# touch **
+# fi
+#
+# exec "${BASH_SOURCE[0]}"
+# fi
# no cpu rationing for now
echo 0-3 > /dev/cpuset/background/cpus
diff --git a/models/driving_model.dlc b/models/driving_model.dlc
index 3cceccf11..d06aad619 100644
Binary files a/models/driving_model.dlc and b/models/driving_model.dlc differ
diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc
index b860a1185..47e71afe2 100644
Binary files a/models/monitoring_model.dlc and b/models/monitoring_model.dlc differ
diff --git a/opendbc/chrysler_pacifica_2017_hybrid.dbc b/opendbc/chrysler_pacifica_2017_hybrid.dbc
index db2892d33..be99769b5 100644
--- a/opendbc/chrysler_pacifica_2017_hybrid.dbc
+++ b/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";
diff --git a/opendbc/hyundai_kia_generic.dbc b/opendbc/hyundai_kia_generic.dbc
index e3e6a1eb6..bb1c06e09 100644
--- a/opendbc/hyundai_kia_generic.dbc
+++ b/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
diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml
index fdafd3da1..d74c1a20e 100644
--- a/panda/.circleci/config.yml
+++ b/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:
diff --git a/panda/VERSION b/panda/VERSION
index 9bdb566fc..87ff52e2b 100644
--- a/panda/VERSION
+++ b/panda/VERSION
@@ -1 +1 @@
-v1.4.1
\ No newline at end of file
+v1.4.2
\ No newline at end of file
diff --git a/panda/board/board.h b/panda/board/board.h
new file mode 100644
index 000000000..5379f9464
--- /dev/null
+++ b/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));
+}
diff --git a/panda/board/board_declarations.h b/panda/board/board_declarations.h
new file mode 100644
index 000000000..7a44113f6
--- /dev/null
+++ b/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;
\ No newline at end of file
diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h
new file mode 100644
index 000000000..0372aa3d0
--- /dev/null
+++ b/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
+};
\ No newline at end of file
diff --git a/panda/board/boards/common.h b/panda/board/boards/common.h
new file mode 100644
index 000000000..d176e4eaf
--- /dev/null
+++ b/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= 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
+};
\ No newline at end of file
diff --git a/panda/board/bootstub.c b/panda/board/bootstub.c
index 799c39d05..9644326d4 100644
--- a/panda/board/bootstub.c
+++ b/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;
diff --git a/panda/board/build.mk b/panda/board/build.mk
index af2bffeb0..acac03095 100644
--- a/panda/board/build.mk
+++ b/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 $^
diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h
index 3db211051..2d7728134 100644
--- a/panda/board/drivers/can.h
+++ b/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
diff --git a/panda/board/drivers/harness.h b/panda/board/drivers/harness.h
new file mode 100644
index 000000000..17520a4ba
--- /dev/null
+++ b/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");
+ }
+}
\ No newline at end of file
diff --git a/panda/board/drivers/llgpio.h b/panda/board/drivers/llgpio.h
index f84ee7f8a..a89c8a8e2 100644
--- a/panda/board/drivers/llgpio.h
+++ b/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));
diff --git a/panda/board/gpio.h b/panda/board/gpio.h
index e1936527e..1acf0288a 100644
--- a/panda/board/gpio.h
+++ b/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; iAHB1ENR |= 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;
}
-}
+}
\ No newline at end of file
diff --git a/panda/board/main.c b/panda/board/main.c
index f96216912..7473b0775 100644
--- a/panda/board/main.c
+++ b/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;
}
-
diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h
new file mode 100644
index 000000000..8929e9ac0
--- /dev/null
+++ b/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;
\ No newline at end of file
diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c
index 21a3a59fa..194370fa3 100644
--- a/panda/board/pedal/main.c
+++ b/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
diff --git a/panda/board/pedal/main_declarations.h b/panda/board/pedal/main_declarations.h
new file mode 100644
index 000000000..9a40f8ae3
--- /dev/null
+++ b/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;
\ No newline at end of file
diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h
index 3a9ae39e4..94ebbb53c 100644
--- a/panda/board/power_saving.h
+++ b/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;
}
diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h
index 1f44e992a..bbad909f2 100644
--- a/panda/board/safety/safety_elm327.h
+++ b/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;
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index 6179a5e37..de2153dd3 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -17,7 +17,7 @@ bool honda_alt_brake_msg = false;
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = GET_ADDR(to_push);
- int len = GET_LEN(to_push);
+ //int len = GET_LEN(to_push);
// sample speed
if (addr == 0x158) {
@@ -60,7 +60,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
- if ((addr == 0x201) && (len == 6)) {
+ if (false) {
gas_interceptor_detected = 1;
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
@@ -72,7 +72,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press if no interceptor
- if (!gas_interceptor_detected) {
+ if (false) {
if (addr == 0x17C) {
int gas = GET_BYTE(to_push, 0);
if (gas && !(honda_gas_prev) && long_controls_allowed) {
@@ -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;
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index c1ce99605..2e6f275d4 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -67,7 +67,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of interceptor gas press
- if (addr == 0x201) {
+ if (false) {
gas_interceptor_detected = 1;
int gas_interceptor = GET_INTERCEPTOR(to_push);
if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
@@ -79,7 +79,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press
- if (addr == 0x2C1) {
+ if (false) {
int gas = GET_BYTE(to_push, 6) & 0xFF;
if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected && long_controls_allowed) {
controls_allowed = 0;
diff --git a/panda/board/spi_flasher.h b/panda/board/spi_flasher.h
index aacea822c..4eab35671 100644
--- a/panda/board/spi_flasher.h
+++ b/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);
}
}
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index bfca642e8..e83a4a169 100644
--- a/panda/python/__init__.py
+++ b/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'')
diff --git a/panda/tests/automated/2_usb_to_can.py b/panda/tests/automated/2_usb_to_can.py
index 7860d3290..9e3e07aa4 100644
--- a/panda/tests/automated/2_usb_to_can.py
+++ 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)
diff --git a/panda/tests/automated/4_wifi_functionality.py b/panda/tests/automated/4_wifi_functionality.py
index 0cf42d1f3..ab9bed700 100644
--- a/panda/tests/automated/4_wifi_functionality.py
+++ b/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)
diff --git a/panda/tests/automated/6_two_panda.py b/panda/tests/automated/6_two_panda.py
index 3c29a0e7a..09cf1861f 100644
--- a/panda/tests/automated/6_two_panda.py
+++ b/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)
diff --git a/panda/tests/black_loopback_test.py b/panda/tests/black_loopback_test.py
new file mode 100755
index 000000000..8683561a4
--- /dev/null
+++ b/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)
diff --git a/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py
index 1345065cb..888bd36e9 100644
--- a/panda/tests/safety/libpandasafety_py.py
+++ b/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);
diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c
index dc3de5504..7cd9b86d8 100644
--- a/panda/tests/safety/test.c
+++ b/panda/tests/safety/test.c
@@ -1,5 +1,6 @@
#include
#include
+#include
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;
diff --git a/panda/tests/safety/test.sh b/panda/tests/safety/test.sh
index 83d8f5b31..2674281ad 100755
--- a/panda/tests/safety/test.sh
+++ b/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
diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py
index 5833d96e3..f16030843 100755
--- a/panda/tests/safety/test_honda.py
+++ b/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
diff --git a/panda/tests/safety/test_honda_bosch.py b/panda/tests/safety/test_honda_bosch.py
index 11c939140..0d37cbe80 100755
--- a/panda/tests/safety/test_honda_bosch.py
+++ b/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)))
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 779ca7b27..f4ca033f5 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -59,7 +59,8 @@ pthread_mutex_t usb_lock;
bool spoofing_started = false;
bool fake_send = false;
bool loopback_can = false;
-bool is_grey_panda = false;
+cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
+bool is_pigeon = false;
pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
@@ -69,6 +70,29 @@ void pigeon_init();
void *pigeon_thread(void *crap);
void *safety_setter_thread(void *s) {
+ char *value_vin;
+ size_t value_vin_sz = 0;
+
+ // switch to no_output when CarVin param is read
+ while (1) {
+ if (do_exit) return NULL;
+ const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz);
+ if (value_vin_sz > 0) {
+ // sanity check VIN format
+ assert(value_vin_sz == 17);
+ break;
+ }
+ usleep(100*1000);
+ }
+ LOGW("got CarVin %s", value_vin);
+
+ pthread_mutex_lock(&usb_lock);
+
+ // VIN qury done, stop listening to OBDII
+ libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
+
+ pthread_mutex_unlock(&usb_lock);
+
char *value;
size_t value_sz = 0;
@@ -151,7 +175,7 @@ void *safety_setter_thread(void *s) {
// must be called before threads or with mutex
bool usb_connect() {
int err;
- unsigned char is_pigeon[1] = {0};
+ unsigned char hw_query[1] = {0};
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { goto fail; }
@@ -184,11 +208,12 @@ bool usb_connect() {
assert(err == 0);
}
- libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, is_pigeon, 1, TIMEOUT);
+ libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT);
- if (is_pigeon[0]) {
- LOGW("grey panda detected");
- is_grey_panda = true;
+ hw_type = (cereal::HealthData::HwType)(hw_query[0]);
+ is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA);
+ if (is_pigeon) {
+ LOGW("panda with gps detected");
pigeon_needs_init = true;
if (pigeon_thread_handle == -1) {
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
@@ -280,12 +305,13 @@ void can_health(void *s) {
struct __attribute__((packed)) health {
uint32_t voltage;
uint32_t current;
- uint8_t started;
- uint8_t controls_allowed;
- uint8_t gas_interceptor_detected;
uint32_t can_send_errs;
uint32_t can_fwd_errs;
uint32_t gmlan_send_errs;
+ uint8_t started;
+ uint8_t controls_allowed;
+ uint8_t gas_interceptor_detected;
+ uint8_t car_harness_status_pkt;
} health;
// recv from board
@@ -293,7 +319,9 @@ void can_health(void *s) {
do {
cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT);
- if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); }
+ if (cnt != sizeof(health)) {
+ handle_usb_issue(cnt, __func__);
+ }
} while(cnt != sizeof(health));
pthread_mutex_unlock(&usb_lock);
@@ -314,15 +342,23 @@ void can_health(void *s) {
}
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
- healthData.setIsGreyPanda(is_grey_panda);
+ healthData.setHasGps(is_pigeon);
healthData.setCanSendErrs(health.can_send_errs);
healthData.setCanFwdErrs(health.can_fwd_errs);
healthData.setGmlanSendErrs(health.gmlan_send_errs);
+ healthData.setHwType(hw_type);
// send to health
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
+
+ pthread_mutex_lock(&usb_lock);
+
+ // send heartbeat back to panda
+ libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT);
+
+ pthread_mutex_unlock(&usb_lock);
}
@@ -447,10 +483,10 @@ void *can_health_thread(void *crap) {
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8011");
- // run at 1hz
+ // run at 2hz
while (!do_exit) {
can_health(publisher);
- usleep(1000*1000);
+ usleep(500*1000);
}
return NULL;
}
@@ -502,7 +538,7 @@ void pigeon_set_baud(int baud) {
void pigeon_init() {
usleep(1000*1000);
- LOGW("grey panda start");
+ LOGW("panda GPS start");
// power off pigeon
pigeon_set_power(0);
@@ -543,7 +579,7 @@ void pigeon_init() {
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
- LOGW("grey panda is ready to fly");
+ LOGW("panda GPS on");
}
static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {
diff --git a/selfdrive/can/tests/test_packer_honda.py b/selfdrive/can/tests/test_packer_honda.py
index 87d62244e..b1744155b 100644
--- a/selfdrive/can/tests/test_packer_honda.py
+++ b/selfdrive/can/tests/test_packer_honda.py
@@ -16,6 +16,9 @@ class TestPackerMethods(unittest.TestCase):
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
+ is_panda_black = False
+ car_fingerprint = HONDA_BOSCH[0]
+
apply_brake = (random.randint(0, 2) % 2 == 0)
pump_on = (random.randint(0, 2) % 2 == 0)
pcm_override = (random.randint(0, 2) % 2 == 0)
@@ -23,33 +26,31 @@ class TestPackerMethods(unittest.TestCase):
chime = random.randint(0, 65536)
fcw = random.randint(0, 65536)
idx = random.randint(0, 65536)
- m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
- m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
+ m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
+ m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
lkas_active = (random.randint(0, 2) % 2 == 0)
- car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
- m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx)
- m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx)
+ m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
+ m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
self.assertEqual(m_old, m)
pcm_speed = random.randint(0, 65536)
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536),
random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
- car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
is_metric = (random.randint(0, 2) % 2 == 0)
- m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx)
- m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx)
+ m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
+ m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
self.assertEqual(m_old, m)
button_val = random.randint(0, 65536)
idx = random.randint(0, 65536)
- m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx)
- m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx)
+ m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black)
+ m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black)
self.assertEqual(m_old, m)
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index 1ce6ed47a..d74892993 100644
--- a/selfdrive/car/car_helpers.py
+++ b/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
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index 24acc76a4..6368847c8 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -27,7 +27,7 @@ def get_can_parser(CP):
("DOOR_OPEN_RL", "DOORS", 0),
("DOOR_OPEN_RR", "DOORS", 0),
("BRAKE_PRESSED_2", "BRAKE_2", 0),
- ("ACCEL_PEDAL", "ACCEL_PEDAL_MSG", 0),
+ ("ACCEL_134", "ACCEL_GAS_134", 0),
("SPEED_LEFT", "SPEED_1", 0),
("SPEED_RIGHT", "SPEED_1", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
@@ -112,7 +112,7 @@ class CarState(object):
self.seatbelt = (cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
- self.pedal_gas = cp.vl["ACCEL_PEDAL_MSG"]['ACCEL_PEDAL']
+ self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
self.car_gas = self.pedal_gas
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index 65a60904f..6cc4061d6 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -38,13 +38,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.chrysler
@@ -94,7 +95,7 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
- ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
+ ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
print("ECU Camera Simulated: {0}".format(ret.enableCamera))
ret.openpilotLongitudinalControl = False
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index 188931a06..f13288154 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -44,7 +44,7 @@ FINGERPRINTS = {
],
CAR.JEEP_CHEROKEE: [
# JEEP GRAND CHEROKEE V6 2018
- {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
+ {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
# Jeep Grand Cherokee 2017 Trailhawk
{257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
],
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index ca0f9f830..59acc8432 100755
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -38,13 +38,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.ford
@@ -87,7 +88,7 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
- ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint)
+ ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = False
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 71b0efad2..449b8ae5d 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -46,19 +46,20 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "gm"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.enableCruise = False
# Presence of a camera on the object bus is ok.
# Have to go to read_only if ASCM is online (ACC-enabled cars),
# or camera is on powertrain bus (LKA cars without ACC).
- ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
+ ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = ret.enableCamera
tire_stiffness_factor = 0.444 # not optimized yet
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index c90c1e4e4..cd4e8a8fa 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/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("DragonTempDisableSteerOnSignal") == "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.
@@ -182,8 +193,18 @@ class CarController(object):
idx = frame // 2
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
+ # DragonAllowGas
+ # if we detect gas pedal pressed, we do not want OP to apply gas or brake
+ # gasPressed code from interface.py
+ if not CS.CP.enableGasInterceptor:
+ gasPressed = CS.pedal_gas > 0
+ else:
+ gasPressed = CS.user_gas_pressed
+ 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:
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 6ca8aa13e..9bf569926 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -154,7 +154,8 @@ def get_can_signals(CP):
def get_can_parser(CP):
signals, checks = get_can_signals(CP)
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+ bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt)
def get_cam_can_parser(CP):
@@ -165,9 +166,8 @@ def get_cam_can_parser(CP):
if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]:
checks = [(0x194, 100)]
- cam_bus = 1 if CP.carFingerprint in HONDA_BOSCH else 2
-
- return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, cam_bus)
+ bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
class CarState(object):
def __init__(self, CP):
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index d323afc8b..bf5651b7c 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -19,7 +19,15 @@ def fix(msg, addr):
return msg2
-def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
+def get_pt_bus(car_fingerprint, is_panda_black):
+ return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0
+
+
+def get_lkas_cmd_bus(car_fingerprint, is_panda_black):
+ return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0
+
+
+def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx, car_fingerprint, is_panda_black):
# TODO: do we loose pressure if we keep pump off for long?
brakelights = apply_brake > 0
brake_rq = apply_brake > 0
@@ -38,27 +46,25 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
# TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
"FCW": fcw << 1,
}
- return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
+ bus = get_pt_bus(car_fingerprint, is_panda_black)
+ return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
-def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
+def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
"STEER_TORQUE_REQUEST": lkas_active,
}
- # Set bus 2 for accord and new crv.
- bus = 2 if car_fingerprint in HONDA_BOSCH else 0
+ bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
-def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
+def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black):
commands = []
- bus = 0
+ bus_pt = get_pt_bus(car_fingerprint, is_panda_black)
+ bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
- # Bosch sends commands to bus 2.
- if car_fingerprint in HONDA_BOSCH:
- bus = 2
- else:
+ if car_fingerprint not in HONDA_BOSCH:
acc_hud_values = {
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
'PCM_GAS': hud.pcm_accel,
@@ -70,7 +76,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx):
'SET_ME_X01_2': 1,
'SET_ME_X01': 1,
}
- commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
+ commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
lkas_hud_values = {
'SET_ME_X41': 0x41,
@@ -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)
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 0d0bbd669..e5b9cd3ad 100755
--- a/selfdrive/car/honda/interface.py
+++ b/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("DragonTempDisableSteerOnSignal") == "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]))
@@ -504,13 +517,18 @@ class CarInterface(object):
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
- # 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]))
+ # 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]))
+ 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]))
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 4bdeca7f0..fa9e068ab 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -73,6 +73,9 @@ class CAR:
PILOT_2019 = "HONDA PILOT 2019 ELITE"
RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION"
+# diag message that in some Nidec cars only appear with 1s freq if VIN query is performed
+DIAG_MSGS = {1600: 5, 1601: 8}
+
FINGERPRINTS = {
CAR.ACCORD: [{
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
@@ -143,6 +146,12 @@ FINGERPRINTS = {
}]
}
+# add DIAG_MSGS to fingerprints
+for c in FINGERPRINTS:
+ for f, _ in enumerate(FINGERPRINTS[c]):
+ for d in DIAG_MSGS:
+ FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
+
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index 5a895497a..d9e7cf3a9 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/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,
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 4e1b2e6be..c367324da 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -40,13 +40,14 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
@@ -141,7 +142,7 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
- ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
+ ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = False
ret.steerLimitAlert = False
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index 3f47b9b00..b703776c0 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -4,7 +4,6 @@ from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
-from common.realtime import Ratekeeper
# mocked car interface to work with chffrplus
TS = 0.01 # 100Hz
@@ -30,8 +29,6 @@ class CarInterface(object):
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
- self.rk = Ratekeeper(100, print_delay_threshold=2. / 1000)
-
@staticmethod
def compute_gb(accel, speed):
return accel
@@ -41,7 +38,7 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
@@ -81,8 +78,6 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
- self.rk.keep_time()
-
# get basic data from phone and gps since CAN isn't connected
sensors = messaging.recv_sock(self.sensor)
if sensors is not None:
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index ad8d1d5a4..c5fe0062a 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -38,12 +38,13 @@ class CarInterface(object):
return 1.0
@staticmethod
- def get_params(candidate, fingerprint, vin=""):
+ def get_params(candidate, fingerprint, vin="", is_panda_black=False):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.carFingerprint = candidate
ret.carVin = vin
+ ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.subaru
ret.enableCruise = True
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 242d9cd85..15df9a30b 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/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("DragonTempDisableSteerOnSignal") == "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)
@@ -229,6 +251,19 @@ class CarController(object):
elif ECU.APGS in self.fake_ecus:
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
+ # DragonAllowGas
+ # if we detect gas pedal pressed, we do not want OP to apply gas or brake
+ # gasPressed code from interface.py
+ if CS.CP.enableGasInterceptor:
+ # use interceptor values to disengage on pedal press
+ gasPressed = CS.pedal_gas > 15
+ else:
+ gasPressed = CS.pedal_gas > 0
+
+ if self.dragon_allow_gas and gasPressed:
+ apply_accel = 0
+ apply_gas = 0
+
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 40be422db..171ff611f 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/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
\ No newline at end of file
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 1b2220cea..062eb28b2 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/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("DragonTempDisableSteerOnSignal") == "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,13 +385,19 @@ class CarInterface(object):
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
- # 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]))
+ 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
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index b5b38bafd..6c0cd006f 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,8 +1,9 @@
#ifndef MODELDATA_H
#define MODELDATA_H
-#define MODEL_PATH_DISTANCE 100
+#define MODEL_PATH_DISTANCE 192
#define POLYFIT_DEGREE 4
+#define SPEED_PERCENTILES 10
typedef struct PathData {
float points[MODEL_PATH_DISTANCE];
@@ -16,8 +17,12 @@ typedef struct LeadData {
float dist;
float prob;
float std;
+ float rel_y;
+ float rel_y_std;
float rel_v;
float rel_v_std;
+ float rel_a;
+ float rel_a_std;
} LeadData;
typedef struct ModelData {
@@ -25,6 +30,8 @@ typedef struct ModelData {
PathData left_lane;
PathData right_lane;
LeadData lead;
+ LeadData lead_future;
+ float speed[SPEED_PERCENTILES];
} ModelData;
#endif
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 7bbcf5fad..723b06d92 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -184,6 +184,9 @@ int read_db_all(const char* params_path, std::map *par
while ((de = readdir(d))) {
if (!isalnum(de->d_name[0])) continue;
std::string key = std::string(de->d_name);
+
+ if (key == "AccessToken") continue;
+
std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str()));
(*params)[key] = value;
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 5c21a9db8..1148fbd95 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.6.1-release"
+#define COMMA_VERSION "0.6.2-release"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 5c5ba2f66..27d4cb69a 100755
--- a/selfdrive/controls/controlsd.py
+++ b/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):
@@ -341,7 +346,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"vEgo": CS.vEgo,
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
- "curvature": VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo),
+ "curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
@@ -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
diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py
index cac491e92..2de4dd010 100644
--- a/selfdrive/controls/lib/alerts.py
+++ b/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",
diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py
index cb7b34fe1..47c782401 100644
--- a/selfdrive/controls/lib/driver_monitor.py
+++ b/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 params.get("DragonDisableDriverSafetyCheck") == "0" and alert is not None:
+ if alert is not None and self.dragon_enable_driver_safety_check:
events.append(create_event(alert, [ET.WARNING]))
return events
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index cbd144edc..190f7c843 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -126,6 +126,7 @@ class PathPlanner(object):
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
+ plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
self.plan.send(plan_send.to_bytes())
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 94cc2fc13..2e01a8f4f 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -1,9 +1,9 @@
#!/usr/bin/env python
-import zmq
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
@@ -16,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
@@ -38,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)
@@ -58,14 +68,12 @@ 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):
def __init__(self, CP, fcw_enabled):
self.CP = CP
- self.poller = zmq.Poller()
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
@@ -81,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:
@@ -110,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
@@ -129,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:
@@ -236,10 +244,6 @@ class Planner(object):
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
- plan_send.plan.vCurvature = v_curvature
- plan_send.plan.decelForTurn = decel_for_turn
- plan_send.plan.mapValid = map_valid
-
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py
index 7efb9f334..6b3d768c7 100644
--- a/selfdrive/controls/lib/radar_helpers.py
+++ b/selfdrive/controls/lib/radar_helpers.py
@@ -1,48 +1,35 @@
-import numpy as np
-
-from common.numpy_fast import clip, interp
+from common.realtime import DT_MDL
from common.kalman.simple_kalman import KF1D
+from selfdrive.config import RADAR_TO_CENTER
+
+# the longer lead decels, the more likely it will keep decelerating
+# TODO is this a good default?
_LEAD_ACCEL_TAU = 1.5
-NO_FUSION_SCORE = 100 # bad default fusion score
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
-rate, ratev = 20., 20. # model and radar are both at 20Hz
-ts = 1./rate
-freq_v_lat = 0.2 # Hz
-k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
-
-freq_a_lead = .5 # Hz
-k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
-
# stationary qualification parameters
-v_stationary_thr = 4. # objects moving below this speed are classified as stationary
-v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
v_ego_stationary = 4. # no stationary object flag below this speed
# Lead Kalman Filter params
-_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
+_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]]
_VLEAD_C = [1.0, 0.0]
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
-_VLEAD_K = [[ 0.1988689 ], [ 0.28555364]]
-
-RDR_TO_LDR = 2.7
+_VLEAD_K = [[0.1988689], [0.28555364]]
class Track(object):
def __init__(self):
self.ekf = None
- self.stationary = True
self.initted = False
- def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override):
+ def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
if self.initted:
# pylint: disable=access-member-before-definition
- self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
self.vRelPrev = self.vRel
@@ -52,9 +39,6 @@ class Track(object):
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
- # compute distance to path
- self.dPath = d_path
-
# computed velocity and accelerations
self.vLead = self.vRel + v_ego_t_aligned
@@ -64,21 +48,8 @@ class Track(object):
self.cnt = 1
self.vision_cnt = 0
self.vision = False
- self.aRel = 0. # nidec gives no information about this
- self.vLat = 0.
self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
- # estimate acceleration
- # TODO: use Kalman filter
- a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
- a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
- self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
-
- # TODO: use Kalman filter
- # neglect steer override cases as dPath is too noisy
- v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts
- self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
-
self.kf.update(self.vLead)
self.cnt += 1
@@ -86,33 +57,12 @@ class Track(object):
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
- if self.stationary:
- # stationary objects can become non stationary, but not the other way around
- self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
- self.oncoming = self.vLead < v_oncoming_thr
-
- self.vision_score = NO_FUSION_SCORE
-
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau = _LEAD_ACCEL_TAU
else:
self.aLeadTau *= 0.9
- def update_vision_score(self, dist_to_vision, rel_speed_diff):
- # rel speed is very hard to estimate from vision
- if dist_to_vision < 4.0 and rel_speed_diff < 10.:
- self.vision_score = dist_to_vision + rel_speed_diff
- else:
- self.vision_score = NO_FUSION_SCORE
-
- def update_vision_fusion(self):
- # vision point is never stationary
- # don't trust 1 or 2 fusions until model quality is much better
- if self.vision_cnt >= 3:
- self.vision = True
- self.stationary = False
-
def get_key_for_cluster(self):
# Weigh y higher since radar is inaccurate in this dimension
return [self.dRel, self.yRel*2, self.vRel]
@@ -171,89 +121,47 @@ class Cluster(object):
def aLeadTau(self):
return mean([t.aLeadTau for t in self.tracks])
- @property
- def vision(self):
- return any([t.vision for t in self.tracks])
-
@property
def measured(self):
return any([t.measured for t in self.tracks])
- @property
- def vision_cnt(self):
- return max([t.vision_cnt for t in self.tracks])
-
- @property
- def stationary(self):
- return all([t.stationary for t in self.tracks])
-
- @property
- def oncoming(self):
- return all([t.oncoming for t in self.tracks])
-
- def toRadarState(self):
+ def get_RadarState(self, model_prob=0.0):
return {
- "dRel": float(self.dRel) - RDR_TO_LDR,
+ "dRel": float(self.dRel),
"yRel": float(self.yRel),
"vRel": float(self.vRel),
- "aRel": float(self.aRel),
"vLead": float(self.vLead),
- "dPath": float(self.dPath),
- "vLat": float(self.vLat),
"vLeadK": float(self.vLeadK),
"aLeadK": float(self.aLeadK),
"status": True,
- "fcw": self.is_potential_fcw(),
+ "fcw": self.is_potential_fcw(model_prob),
+ "modelProb": model_prob,
+ "radar": True,
"aLeadTau": float(self.aLeadTau)
}
+ def get_RadarState_from_vision(self, lead_msg, v_ego):
+ return {
+ "dRel": float(lead_msg.dist - RADAR_TO_CENTER),
+ "yRel": float(lead_msg.relY),
+ "vRel": float(lead_msg.relVel),
+ "vLead": float(v_ego + lead_msg.relVel),
+ "vLeadK": float(v_ego + lead_msg.relVel),
+ "aLeadK": float(0),
+ "aLeadTau": _LEAD_ACCEL_TAU,
+ "fcw": False,
+ "modelProb": float(lead_msg.prob),
+ "radar": False,
+ "status": True
+ }
+
def __str__(self):
- ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath)
- if self.stationary:
- ret += " stationary"
- if self.vision:
- ret += " vision"
- if self.oncoming:
- ret += " oncoming"
- if self.vision_cnt > 0:
- ret += " vision_cnt: %6.0f" % self.vision_cnt
+ ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f" % (self.dRel, self.yRel, self.vRel, self.aLeadK)
return ret
- def is_potential_lead(self, v_ego):
- # predict cut-ins by extrapolating lateral speed by a lookahead time
- # lookahead time depends on cut-in distance. more attentive for close cut-ins
- # also, above 50 meters the predicted path isn't very reliable
+ 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
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 306dab05b..9286ea058 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -37,8 +37,6 @@ def plannerd_thread():
sm['liveParameters'].sensorValid = True
sm['liveParameters'].steerRatio = CP.steerRatio
sm['liveParameters'].stiffnessFactor = 1.0
- live_map_data = messaging.new_message()
- live_map_data.init('liveMapData')
while True:
sm.update()
@@ -46,9 +44,7 @@ def plannerd_thread():
if sm.updated['model']:
PP.update(sm, CP, VM)
if sm.updated['radarState']:
- PL.update(sm, CP, VM, PP, live_map_data.liveMapData)
- # elif socket is live_map_data_sock:
- # live_map_data = msg
+ PL.update(sm, CP, VM, PP)
def main(gctx=None):
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index afb4ffd2b..77e9febbc 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -6,18 +6,13 @@ from collections import defaultdict, deque
import selfdrive.messaging as messaging
from selfdrive.services import service_list
-from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
-from selfdrive.controls.lib.model_parser import ModelParser
-from selfdrive.controls.lib.radar_helpers import Track, Cluster, \
- RDR_TO_LDR, NO_FUSION_SCORE
-
+from selfdrive.controls.lib.radar_helpers import Track, Cluster
+from selfdrive.config import RADAR_TO_CENTER
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
-from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
from common.params import Params
from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL
-from common.kalman.ekf import EKF, SimpleSensor
DEBUG = False
@@ -26,106 +21,94 @@ DIMSV = 2
XV, SPEEDV = 0, 1
VISION_POINT = -1
-path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
-
# Time-alignment
rate = 1. / DT_MDL # model and radar are both at 20Hz
v_len = 20 # how many speed data points to remember for t alignment with rdr data
-class EKFV1D(EKF):
- def __init__(self):
- super(EKFV1D, self).__init__(False)
- self.identity = numpy.matlib.identity(DIMSV)
- self.state = np.matlib.zeros((DIMSV, 1))
- self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point
- self.covar = self.identity * self.var_init
- self.process_noise = np.matlib.diag([0.5, 1])
+def laplacian_cdf(x, mu, b):
+ b = np.max([b, 1e-4])
+ return np.exp(-abs(x-mu)/b)
+
+
+def match_vision_to_cluster(v_ego, lead, clusters):
+ # match vision point to best statistical cluster match
+ probs = []
+ offset_vision_dist = lead.dist - RADAR_TO_CENTER
+ for c in clusters:
+ prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
+ prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
+ prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
+ # This is isn't exactly right, but good heuristic
+ combined_prob = prob_d * prob_y * prob_v
+ probs.append(combined_prob)
+ idx = np.argmax(probs)
+ # if no 'sane' match is found return -1
+ # stationary radar points can be false positives
+ dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+ vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2)
+ if dist_sane and vel_sane:
+ return idx
+ else:
+ return None
+
+
+def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
+ # Determine leads, this is where the essential logic happens
+ if len(clusters) > 0 and ready and lead_msg.prob > .5:
+ lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters)
+ else:
+ lead_idx = None
+
+ lead_dict = {'status': False}
+ if lead_idx is not None:
+ lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob)
+ elif (lead_idx is None) and ready and (lead_msg.prob > .5):
+ lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
+
+ if low_speed_override:
+ low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
+ if len(low_speed_clusters) > 0:
+ lead_idx = np.argmin([c.dRel for c in low_speed_clusters])
+ if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']):
+ lead_dict = low_speed_clusters[lead_idx].get_RadarState()
+
+ return lead_dict
- def calc_transfer_fun(self, dt):
- tf = np.matlib.identity(DIMSV)
- tf[XV, SPEEDV] = dt
- tfj = tf
- return tf, tfj
class RadarD(object):
- def __init__(self, VM, mocked):
- self.VM = VM
+ def __init__(self, mocked):
+ self.current_time = 0
self.mocked = mocked
- self.MP = ModelParser()
self.tracks = defaultdict(dict)
self.last_md_ts = 0
self.last_controls_state_ts = 0
self.active = 0
- self.steer_angle = 0.
- self.steer_override = False
-
- # Kalman filter stuff:
- self.ekfv = EKFV1D()
- self.speedSensorV = SimpleSensor(XV, 1, 2)
# v_ego
self.v_ego = 0.
self.v_ego_hist_t = deque([0], maxlen=v_len)
self.v_ego_hist_v = deque([0], maxlen=v_len)
self.v_ego_t_aligned = 0.
+ self.ready = False
- def update(self, frame, delay, sm, rr):
- ar_pts = {}
- for pt in rr.points:
- ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
-
- if sm.updated['liveParameters']:
- self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
+ def update(self, frame, delay, sm, rr, has_radar):
+ self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()])
if sm.updated['controlsState']:
self.active = sm['controlsState'].active
self.v_ego = sm['controlsState'].vEgo
- self.steer_angle = sm['controlsState'].angleSteers
- self.steer_override = sm['controlsState'].steerOverride
-
self.v_ego_hist_v.append(self.v_ego)
self.v_ego_hist_t.append(float(frame)/rate)
-
- self.last_controls_state_ts = sm.logMonoTime['controlsState']
-
if sm.updated['model']:
- self.last_md_ts = sm.logMonoTime['model']
- self.MP.update(self.v_ego, sm['model'])
+ 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()
diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index 99f889788..30f3b3475 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -1,8 +1,9 @@
"""Install exception handler for process crash."""
import os
import sys
+import json
+#from subprocess import check_output
import threading
-import capnp
from selfdrive.version import version, dirty
from selfdrive.swaglog import cloudlog
@@ -20,24 +21,32 @@ else:
from raven import Client
from raven.transport.http import HTTPTransport
- with open("/data/openpilot/.git/HEAD", "r") as f:
- content = f.read().split(' ')
- branch = content[1]
- error_tags = {'dirty': dirty, 'branch': branch}
+ error_tags = {'dirty': dirty, 'username': 'char_error'}
+
try:
with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f:
auth = json.loads(f.read())
auth = json.loads(auth['commaUser'])
- error_tags['username'] = auth['username']
- error_tags['email'] = auth['email']
+ tags = ['username', 'email']
+ for tag in tags:
+ try:
+ error_tags[tag] = ''.join(char for char in auth[tag].decode('utf-8', 'ignore') if char.isalnum())
+ except:
+ pass
except:
pass
+
+ logging_data = {"branch": "/data/params/d/GitBranch", "commit": "/data/params/d/GitCommit", "remote": "/data/params/d/GitRemote"}
+
+ for key in logging_data:
+ try:
+ with open(logging_data[key], "r") as f:
+ error_tags[key] = str(f.read())
+ except:
+ error_tags[key] = "unknown"
+
client = Client('https://980a0cba712a4c3593c33c78a12446e1:fecab286bcaf4dba8b04f7cff0188e2d@sentry.io/1488600',
install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags)
- try:
- client.user_context(error_tags['username'])
- except:
- pass
def capture_warning(warning_string):
client.captureMessage(warning_string, level='warning')
@@ -46,9 +55,7 @@ else:
client.captureMessage(info_string, level='info')
def capture_exception(*args, **kwargs):
- exc_info = sys.exc_info()
- if not exc_info[0] is capnp.lib.capnp.KjException:
- client.captureException(*args, **kwargs)
+ client.captureException(*args, **kwargs)
cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1))
def bind_user(**kwargs):
diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py
index 3c5ac474e..9fefd3365 100644
--- a/selfdrive/dragonpilot/dragonconf/__init__.py
+++ b/selfdrive/dragonpilot/dragonconf/__init__.py
@@ -1,26 +1,69 @@
#!/usr/bin/env python2.7
-# import os
-# import json
-
-# file = '/data/dragonpilot.json'
-
+from common.params import Params
default_conf = {
'DragonEnableDashcam': '1',
- 'DragonDisableDriverSafetyCheck': '0',
+ 'DragonDisableDriverSafetyCheck': '0', # deprecated
+ 'DragonEnableDriverSafetyCheck': '1',
'DragonAutoShutdownAt': '30', # in minute
- 'DragonTempDisableSteerOnSignal': '0',
- 'DragonDisableLogger': '0',
- 'DragonDisableUploader': '0',
+ 'DragonTempDisableSteerOnSignal': '0', # deprecated
+ 'DragonEnableSteeringOnSignal': '0',
+ 'DragonDisableLogger': '0', # deprecated
+ 'DragonEnableLogger': '1',
+ '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', # deprecated
+ 'DragonToyotaStockDSU': '0',
+ 'DragonLatCtrl': '1',
+ 'DragonUIEvent': '0',
+ 'DragonUIMaxSpeed': '0',
+ 'DragonUIFace': '0',
+ 'DragonUIDev': '0',
+ 'DragonUIDevMini': '1',
+}
+
+deprecated_conf = {
+ 'DragonDisableDriverSafetyCheck': 'DragonEnableDriverSafetyCheck',
+ 'DragonTempDisableSteerOnSignal': 'DragonEnableSteeringOnSignal',
+ 'DragonDisableLogger': 'DragonEnableLogger',
+ 'DragonDisableUploader': 'DragonEnableUploader',
+ 'DragonBBUI': 'DragonUIDev',
+}
+
+deprecated_conf_invert = {
+ 'DragonDisableDriverSafetyCheck': True,
+ '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:
+ 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
for key, val in default_conf.items():
- if params.get(key) is None:
+ if params.get(key) is None and key not in deprecated_conf:
params.put(key, str(val))
+
+if __name__ == "__main__":
+ params = Params()
+ params.manager_start()
+
+ dragonpilot_set_params(params)
\ No newline at end of file
diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc
index ae4b05a71..df2c72489 100644
--- a/selfdrive/locationd/locationd_yawrate.cc
+++ b/selfdrive/locationd/locationd_yawrate.cc
@@ -5,6 +5,8 @@
#include
#include
+#include "cereal/gen/cpp/log.capnp.h"
+
#include "locationd_yawrate.h"
@@ -42,6 +44,9 @@ void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odo
double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
+
+ auto trans = camera_odometry.getTrans();
+ posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
}
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
@@ -65,10 +70,7 @@ Localizer::Localizer() {
R_gyro = pow(0.05, 2.0);
}
-cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t msg_size) {
- const kj::ArrayPtr view((const capnp::word*)msg_dat, msg_size);
- capnp::FlatArrayMessageReader msg(view);
- cereal::Event::Reader event = msg.getRoot();
+void Localizer::handle_log(cereal::Event::Reader event) {
double current_time = event.getLogMonoTime() / 1.0e9;
if (prev_update_time < 0) {
@@ -89,8 +91,6 @@ cereal::Event::Which Localizer::handle_log(const unsigned char* msg_dat, size_t
default:
break;
}
-
- return type;
}
@@ -101,8 +101,12 @@ extern "C" {
}
void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) {
+ const kj::ArrayPtr view((const capnp::word*)data, len);
+ capnp::FlatArrayMessageReader msg(view);
+ cereal::Event::Reader event = msg.getRoot();
+
Localizer * loc = (Localizer*) localizer;
- loc->handle_log(data, len);
+ loc->handle_log(event);
}
double localizer_get_yaw(void * localizer) {
diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h
index 323e87de4..d5db91e79 100644
--- a/selfdrive/locationd/locationd_yawrate.h
+++ b/selfdrive/locationd/locationd_yawrate.h
@@ -25,10 +25,12 @@ public:
Eigen::Vector2d x;
double steering_angle = 0;
double car_speed = 0;
+ double posenet_speed = 0;
double prev_update_time = -1;
double controls_state_time = -1;
double sensor_data_time = -1;
Localizer();
- cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size);
+ void handle_log(cereal::Event::Reader event);
+
};
diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc
index 9fc3ad625..1766db311 100644
--- a/selfdrive/locationd/paramsd.cc
+++ b/selfdrive/locationd/paramsd.cc
@@ -1,3 +1,4 @@
+#include
#include
#include
#include
@@ -63,6 +64,7 @@ int main(int argc, char *argv[]) {
double sR = car_params.getSteerRatio();
double x = 1.0;
double ao = 0.0;
+ double posenet_invalid_count = 0;
if (result == 0){
auto str = std::string(value, value_sz);
@@ -108,14 +110,25 @@ int main(int argc, char *argv[]) {
assert(err == 0);
err = zmq_msg_recv(&msg, polls[i].socket, 0);
assert(err >= 0);
+
// make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
-
- auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size());
zmq_msg_close(&msg);
- if (which == cereal::Event::CONTROLS_STATE){
+ capnp::FlatArrayMessageReader capnp_msg(amsg);
+ cereal::Event::Reader event = capnp_msg.getRoot();
+
+ localizer.handle_log(event);
+
+ auto which = event.which();
+ if (which == cereal::Event::CAMERA_ODOMETRY){
+ if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.5 * localizer.car_speed, 5.0)) {
+ posenet_invalid_count++;
+ } else {
+ posenet_invalid_count = 0;
+ }
+ } else if (which == cereal::Event::CONTROLS_STATE){
save_counter++;
double yaw_rate = -localizer.x[0];
@@ -141,13 +154,14 @@ int main(int argc, char *argv[]) {
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
+ live_params.setPosenetSpeed(localizer.posenet_speed);
+ live_params.setPosenetValid(posenet_invalid_count < 5);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
-
// Save parameters every minute
if (save_counter % 6000 == 0) {
json11::Json json = json11::Json::object {
diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py
index e2a6913c0..b29f591e7 100755
--- a/selfdrive/locationd/test/test_params_learner.py
+++ b/selfdrive/locationd/test/test_params_learner.py
@@ -21,12 +21,12 @@ class TestParamsLearner(unittest.TestCase):
# Setup vehicle model with wrong parameters
VM_sim = VehicleModel(self.CP)
x_target = 0.75
- sr_target = 14
+ sr_target = self.CP.steerRatio - 0.5
ao_target = -1.0
VM_sim.update_params(x_target, sr_target)
# Run simulation
- times = np.arange(0, 10*3600, 0.01)
+ times = np.arange(0, 15*3600, 0.01)
angle_offset = np.radians(ao_target)
steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset
speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 486cacd36..b902e6170 100755
--- a/selfdrive/manager.py
+++ b/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)
@@ -591,11 +597,11 @@ def main():
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
close_fds=True)
- if params.get("DragonDisableLogger") == "1":
+ if params.get("DragonEnableLogger") == "0":
del managed_processes['loggerd']
del managed_processes['tombstoned']
- if params.get("DragonDisableUploader") == "1":
+ if params.get("DragonEnableUploader") == "0":
del managed_processes['uploader']
try:
diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml
index 9d141c88d..0c82a3187 100644
--- a/selfdrive/service_list.yaml
+++ b/selfdrive/service_list.yaml
@@ -11,14 +11,14 @@ sensorEvents: [8003, true, 100., 100]
# GPS data, also global timestamp
gpsNMEA: [8004, true, 9.] # 9 msgs each sec
# CPU+MEM+GPU+BAT temps
-thermal: [8005, true, 1., 1]
+thermal: [8005, true, 2., 1]
# List(CanData), list of can messages
can: [8006, true, 100.]
controlsState: [8007, true, 100., 100]
#liveEvent: [8008, true, 0.]
model: [8009, true, 20.]
features: [8010, true, 0.]
-health: [8011, true, 1., 1]
+health: [8011, true, 2., 1]
radarState: [8012, true, 20.]
#liveUI: [8014, true, 0.]
encodeIdx: [8015, true, 20.]
diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py
index 551fbd0d9..ea5ebdd9e 100644
--- a/selfdrive/test/plant/maneuver.py
+++ b/selfdrive/test/plant/maneuver.py
@@ -1,3 +1,4 @@
+from collections import defaultdict
from selfdrive.test.plant.maneuverplots import ManeuverPlot
from selfdrive.test.plant.plant import Plant
import numpy as np
@@ -16,6 +17,7 @@ class Maneuver(object):
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
+ self.checks = kwargs.get("checks", [])
self.duration = duration
self.title = title
@@ -28,6 +30,7 @@ class Maneuver(object):
distance_lead = self.distance_lead
)
+ logs = defaultdict(list)
last_controls_state = None
plot = ManeuverPlot(self.title)
@@ -42,20 +45,24 @@ class Maneuver(object):
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
- distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
- if controls_state:
- last_controls_state = controls_state[-1]
+ # distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
+ log = plant.step(speed_lead, current_button, grade)
- d_rel = distance_lead - distance if self.lead_relevancy else 200.
- v_rel = speed_lead - speed if self.lead_relevancy else 0.
+ if log['controls_state_msgs']:
+ last_controls_state = log['controls_state_msgs'][-1]
+
+ d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
+ v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
+ log['d_rel'] = d_rel
+ log['v_rel'] = v_rel
if last_controls_state:
# print(last_controls_state)
#develop plots
plot.add_data(
time=plant.current_time(),
- gas=gas, brake=brake, steer_torque=steer_torque,
- distance=distance, speed=speed, acceleration=acceleration,
+ gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
+ distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
uf_accel_cmd=last_controls_state.ufAccelCmd,
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
@@ -63,10 +70,17 @@ class Maneuver(object):
cruise_speed=last_controls_state.vCruise,
jerk_factor=last_controls_state.jerkFactor,
a_target=last_controls_state.aTarget,
- fcw=fcw)
+ 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)
diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py
index 930f83be8..3cd0d6b16 100755
--- a/selfdrive/test/plant/plant.py
+++ b/selfdrive/test/plant/plant.py
@@ -332,13 +332,15 @@ class Plant(object):
live_parameters.init('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.sensorValid = True
+ live_parameters.liveParameters.posenetValid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes())
driver_monitoring = messaging.new_message()
driver_monitoring.init('driverMonitoring')
- driver_monitoring.driverMonitoring.descriptor = [0.] * 7
+ driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3
+ driver_monitoring.driverMonitoring.facePosition = [0.] * 2
Plant.driverMonitoring.send(driver_monitoring.to_bytes())
health = messaging.new_message()
@@ -364,9 +366,26 @@ class Plant(object):
x.points = [0.0]*50
x.prob = 1.0
x.std = 1.0
+
+ if self.lead_relevancy:
+ d_rel = np.maximum(0., distance_lead - distance)
+ v_rel = v_lead - speed
+ prob = 1.0
+ else:
+ d_rel = 200.
+ v_rel = 0.
+ prob = 0.0
+
md.model.lead.dist = float(d_rel)
- md.model.lead.prob = 1.
- md.model.lead.std = 0.1
+ md.model.lead.prob = prob
+ md.model.lead.relY = 0.0
+ md.model.lead.relYStd = 1.
+ md.model.lead.relVel = float(v_rel)
+ md.model.lead.relVelStd = 1.
+ md.model.lead.relA = 0.0
+ md.model.lead.relAStd = 10.
+ md.model.lead.std = 1.0
+
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
@@ -383,7 +402,17 @@ class Plant(object):
self.distance_lead_prev = distance_lead
self.rk.keep_time()
- return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state_msgs)
+ return {
+ "distance": distance,
+ "speed": speed,
+ "acceleration": acceleration,
+ "distance_lead": distance_lead,
+ "brake": brake,
+ "gas": gas,
+ "steer_torque": steer_torque,
+ "fcw": fcw,
+ "controls_state_msgs": controls_state_msgs,
+ }
# simple engage in standalone mode
def plant_thread(rate=100):
diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py
index 9806f5a19..09e81b2a1 100755
--- a/selfdrive/test/tests/plant/test_longitudinal.py
+++ b/selfdrive/test/tests/plant/test_longitudinal.py
@@ -22,22 +22,34 @@ def create_dir(path):
except OSError:
pass
+
+def check_no_collision(log):
+ return min(log['d_rel']) > 0
+
+def check_fcw(log):
+ return any(log['fcw'])
+
+def check_engaged(log):
+ return log['controls_state_msgs'][-1][-1].active
+
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
- (CB.RES_ACCEL, 10.), (0, 10.1),
- (CB.RES_ACCEL, 10.2), (0, 10.3)]
+ (CB.RES_ACCEL, 10.), (0, 10.1),
+ (CB.RES_ACCEL, 10.2), (0, 10.3)],
+ checks=[check_engaged],
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
- (CB.DECEL_SET, 10.), (0, 10.1),
- (CB.DECEL_SET, 10.2), (0, 10.3)]
+ (CB.DECEL_SET, 10.), (0, 10.1),
+ (CB.DECEL_SET, 10.2), (0, 10.3)],
+ checks=[check_engaged],
),
Maneuver(
'while cruising at 20mph, grade change +10%',
@@ -45,7 +57,8 @@ maneuvers = [
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
- grade_breakpoints = [0., 10., 11.]
+ grade_breakpoints = [0., 10., 11.],
+ checks=[check_engaged],
),
Maneuver(
'while cruising at 20mph, grade change -10%',
@@ -53,7 +66,8 @@ maneuvers = [
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
- grade_breakpoints = [0., 10., 11.]
+ grade_breakpoints = [0., 10., 11.],
+ checks=[check_engaged],
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
@@ -63,7 +77,8 @@ maneuvers = [
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
@@ -73,7 +88,8 @@ maneuvers = [
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
@@ -83,7 +99,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 35.0],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@@ -93,7 +110,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 25.0],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@@ -103,7 +121,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 21.66],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_fcw],
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
@@ -113,7 +132,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values = [20., 20., 0.],
speed_lead_breakpoints = [0., 15., 19.],
- cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
+ cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
+ checks=[check_engaged, check_fcw],
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
@@ -122,9 +142,10 @@ maneuvers = [
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
@@ -135,8 +156,9 @@ maneuvers = [
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
@@ -147,8 +169,9 @@ maneuvers = [
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
@@ -159,11 +182,12 @@ maneuvers = [
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+ (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+ (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
@@ -175,7 +199,8 @@ maneuvers = [
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
@@ -186,8 +211,9 @@ maneuvers = [
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
@@ -198,11 +224,12 @@ maneuvers = [
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+ (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+ (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
@@ -213,11 +240,12 @@ maneuvers = [
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
- (CB.RES_ACCEL, 1.4), (0.0, 1.5),
- (CB.RES_ACCEL, 1.6), (0.0, 1.7),
- (CB.RES_ACCEL, 1.8), (0.0, 1.9),
- (CB.RES_ACCEL, 2.0), (0.0, 2.1),
- (CB.RES_ACCEL, 2.2), (0.0, 2.3)]
+ (CB.RES_ACCEL, 1.4), (0.0, 1.5),
+ (CB.RES_ACCEL, 1.6), (0.0, 1.7),
+ (CB.RES_ACCEL, 1.8), (0.0, 1.9),
+ (CB.RES_ACCEL, 2.0), (0.0, 2.1),
+ (CB.RES_ACCEL, 2.2), (0.0, 2.3)],
+ checks=[check_engaged, check_no_collision],
),
Maneuver(
"fcw: traveling at 30 m/s and approaching lead traveling at 20m/s",
@@ -227,7 +255,8 @@ maneuvers = [
initial_distance_lead=100.,
speed_lead_values=[20.],
speed_lead_breakpoints=[1.],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 1m/s2",
@@ -237,7 +266,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 23.],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 3m/s2",
@@ -247,7 +277,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 9.6],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
),
Maneuver(
"fcw: traveling at 20 m/s following a lead that decels from 20m/s to 0 at 5m/s2",
@@ -257,7 +288,8 @@ maneuvers = [
initial_distance_lead=35.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[3., 7.],
- cruise_button_presses = []
+ cruise_button_presses = [],
+ checks=[check_fcw],
)
]
@@ -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)
diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py
index 0032822dc..a087d670f 100755
--- a/selfdrive/thermald.py
+++ b/selfdrive/thermald.py
@@ -8,7 +8,7 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.loggerd.config import get_available_percent
from common.params import Params
-from common.realtime import sec_since_boot
+from common.realtime import sec_since_boot, DT_TRML
from common.numpy_fast import clip
from common.filter_simple import FirstOrderFilter
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),
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index 70cb840d3..ae11cfe06 100644
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@@ -69,6 +69,7 @@ const int viz_w = vwp_w-(bdr_s*2);
const int header_h = 420;
const int footer_h = 280;
const int footer_y = vwp_h-bdr_s-footer_h;
+const int bdr_is = 30;
const int UI_FREQ = 30; // Hz
@@ -127,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;
@@ -293,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;
@@ -662,11 +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");
+ // 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;
+
+ // 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) {
@@ -1165,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) {
@@ -1306,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);
@@ -1418,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) {
@@ -1445,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));
@@ -1490,6 +1529,209 @@ static void ui_draw_infobar(UIState *s) {
nvgText(s->vg, rect_x + 720 + sidebar_offset, rect_y + 35, infobar, NULL);
}
+//BB START: functions added for the display of various items
+static int bb_ui_draw_measure(UIState *s, const char* bb_value, const char* bb_uom, const char* bb_label,
+ int bb_x, int bb_y, int bb_uom_dx,
+ NVGcolor bb_valueColor, NVGcolor bb_labelColor, NVGcolor bb_uomColor,
+ int bb_valueFontSize, int bb_labelFontSize, int bb_uomFontSize ) {
+ const UIScene *scene = &s->scene;
+ nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
+ int dx = 0;
+ if (strlen(bb_uom) > 0) {
+ dx = (int)(bb_uomFontSize*2.5/2);
+ }
+ //print value
+ nvgFontFace(s->vg, "sans-semibold");
+ nvgFontSize(s->vg, bb_valueFontSize*2.5);
+ nvgFillColor(s->vg, bb_valueColor);
+ nvgText(s->vg, bb_x-dx/2, bb_y+ (int)(bb_valueFontSize*2.5)+5, bb_value, NULL);
+ //print label
+ nvgFontFace(s->vg, "sans-regular");
+ nvgFontSize(s->vg, bb_labelFontSize*2.5);
+ nvgFillColor(s->vg, bb_labelColor);
+ nvgText(s->vg, bb_x, bb_y + (int)(bb_valueFontSize*2.5)+5 + (int)(bb_labelFontSize*2.5)+5, bb_label, NULL);
+ //print uom
+ if (strlen(bb_uom) > 0) {
+ nvgSave(s->vg);
+ int rx =bb_x + bb_uom_dx + bb_valueFontSize -3;
+ int ry = bb_y + (int)(bb_valueFontSize*2.5/2)+25;
+ nvgTranslate(s->vg,rx,ry);
+ nvgRotate(s->vg, -1.5708); //-90deg in radians
+ nvgFontFace(s->vg, "sans-regular");
+ nvgFontSize(s->vg, (int)(bb_uomFontSize*2.5));
+ nvgFillColor(s->vg, bb_uomColor);
+ nvgText(s->vg, 0, 0, bb_uom, NULL);
+ nvgRestore(s->vg);
+ }
+ return (int)((bb_valueFontSize + bb_labelFontSize)*2.5) + 5;
+}
+
+static void bb_ui_draw_measures_left(UIState *s, int bb_x, int bb_y, int bb_w ) {
+ const UIScene *scene = &s->scene;
+ int bb_rx = bb_x + (int)(bb_w/2);
+ int bb_ry = bb_y;
+ int bb_h = 5;
+ NVGcolor lab_color = nvgRGBA(255, 255, 255, 200);
+ NVGcolor uom_color = nvgRGBA(255, 255, 255, 200);
+ int value_fontSize=30;
+ int label_fontSize=15;
+ int uom_fontSize = 15;
+ int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ;
+
+ //add visual radar relative distance
+ if (true) {
+ char val_str[16];
+ char uom_str[6];
+ NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
+ if (scene->lead_status) {
+ //show RED if less than 5 meters
+ //show orange if less than 15 meters
+ if((int)(scene->lead_d_rel) < 15) {
+ val_color = nvgRGBA(255, 188, 3, 200);
+ }
+ if((int)(scene->lead_d_rel) < 5) {
+ val_color = nvgRGBA(255, 0, 0, 200);
+ }
+ // lead car relative distance is always in meters
+ snprintf(val_str, sizeof(val_str), "%d", (int)scene->lead_d_rel);
+ } else {
+ snprintf(val_str, sizeof(val_str), "-");
+ }
+ snprintf(uom_str, sizeof(uom_str), "m ");
+ bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL DIST",
+ bb_rx, bb_ry, bb_uom_dx,
+ val_color, lab_color, uom_color,
+ value_fontSize, label_fontSize, uom_fontSize );
+ bb_ry = bb_y + bb_h;
+ }
+
+ //add visual radar relative speed
+ if (true) {
+ char val_str[16];
+ char uom_str[6];
+ NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
+ if (scene->lead_status) {
+ //show Orange if negative speed (approaching)
+ //show Orange if negative speed faster than 5mph (approaching fast)
+ if((int)(scene->lead_v_rel) < 0) {
+ val_color = nvgRGBA(255, 188, 3, 200);
+ }
+ if((int)(scene->lead_v_rel) < -5) {
+ val_color = nvgRGBA(255, 0, 0, 200);
+ }
+ // lead car relative speed is always in meters
+ if (s->is_metric) {
+ snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 3.6 + 0.5));
+ } else {
+ snprintf(val_str, sizeof(val_str), "%d", (int)(scene->lead_v_rel * 2.2374144 + 0.5));
+ }
+ } else {
+ snprintf(val_str, sizeof(val_str), "-");
+ }
+ if (s->is_metric) {
+ snprintf(uom_str, sizeof(uom_str), "km/h");;
+ } else {
+ snprintf(uom_str, sizeof(uom_str), "mph");
+ }
+ bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REL SPEED",
+ bb_rx, bb_ry, bb_uom_dx,
+ val_color, lab_color, uom_color,
+ value_fontSize, label_fontSize, uom_fontSize );
+ bb_ry = bb_y + bb_h;
+ }
+
+ //finally draw the frame
+ bb_h += 20;
+ nvgBeginPath(s->vg);
+ nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20);
+ nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80));
+ nvgStrokeWidth(s->vg, 6);
+ nvgStroke(s->vg);
+}
+
+static void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) {
+ const UIScene *scene = &s->scene;
+ int bb_rx = bb_x + (int)(bb_w/2);
+ int bb_ry = bb_y;
+ int bb_h = 5;
+ NVGcolor lab_color = nvgRGBA(255, 255, 255, 200);
+ NVGcolor uom_color = nvgRGBA(255, 255, 255, 200);
+ int value_fontSize=30;
+ int label_fontSize=15;
+ int uom_fontSize = 15;
+ int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ;
+
+ //add steering angle
+ if (true) {
+ char val_str[16];
+ char uom_str[6];
+ NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
+ //show Orange if more than 6 degrees
+ //show red if more than 12 degrees
+ if(((int)(scene->angleSteers) < -6) || ((int)(scene->angleSteers) > 6)) {
+ val_color = nvgRGBA(255, 188, 3, 200);
+ }
+ if(((int)(scene->angleSteers) < -12) || ((int)(scene->angleSteers) > 12)) {
+ val_color = nvgRGBA(255, 0, 0, 200);
+ }
+ // steering is in degrees
+ snprintf(val_str, sizeof(val_str), "%.0f°",(scene->angleSteers));
+
+ snprintf(uom_str, sizeof(uom_str), "");
+ bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "REAL STEER",
+ bb_rx, bb_ry, bb_uom_dx,
+ val_color, lab_color, uom_color,
+ value_fontSize, label_fontSize, uom_fontSize );
+ bb_ry = bb_y + bb_h;
+ }
+
+ //add desired steering angle
+ if (true) {
+ char val_str[16];
+ char uom_str[6];
+ NVGcolor val_color = nvgRGBA(255, 255, 255, 200);
+ //show Orange if more than 6 degrees
+ //show red if more than 12 degrees
+ if(((int)(scene->angleSteersDes) < -6) || ((int)(scene->angleSteersDes) > 6)) {
+ val_color = nvgRGBA(255, 188, 3, 200);
+ }
+ if(((int)(scene->angleSteersDes) < -12) || ((int)(scene->angleSteersDes) > 12)) {
+ val_color = nvgRGBA(255, 0, 0, 200);
+ }
+ // steering is in degrees
+ snprintf(val_str, sizeof(val_str), "%.0f°",(scene->angleSteersDes));
+
+ snprintf(uom_str, sizeof(uom_str), "");
+ bb_h +=bb_ui_draw_measure(s, val_str, uom_str, "DESIR STEER",
+ bb_rx, bb_ry, bb_uom_dx,
+ val_color, lab_color, uom_color,
+ value_fontSize, label_fontSize, uom_fontSize );
+ bb_ry = bb_y + bb_h;
+ }
+
+ //finally draw the frame
+ bb_h += 20;
+ nvgBeginPath(s->vg);
+ nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20);
+ nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80));
+ nvgStrokeWidth(s->vg, 6);
+ nvgStroke(s->vg);
+}
+
+static void ui_draw_bbui(UIState *s) {
+ const UIScene *scene = &s->scene;
+ const int bb_dml_w = 180;
+ const int bb_dml_x = (scene->ui_viz_rx + (bdr_is * 2));
+ const int bb_dml_y = (box_y + (bdr_is * 1.5)) + 220;
+
+ const int bb_dmr_w = 180;
+ const int bb_dmr_x = scene->ui_viz_rx + scene->ui_viz_rw - bb_dmr_w - (bdr_is * 2);
+ const int bb_dmr_y = (box_y + (bdr_is * 1.5)) + 220;
+
+ bb_ui_draw_measures_right(s, bb_dml_x, bb_dml_y, bb_dml_w);
+ bb_ui_draw_measures_left(s, bb_dmr_x, bb_dmr_y, bb_dmr_w);
+}
+
static void ui_draw_vision_footer(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
@@ -1498,12 +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
- ui_draw_infobar(s);
+ if (s->dragon_ui_dev) {
+ ui_draw_bbui(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,
@@ -1719,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;
@@ -2404,6 +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);
+ // 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);
diff --git a/selfdrive/visiond/models/commonmodel.c b/selfdrive/visiond/models/commonmodel.c
index c2bb657bb..33aabd1ce 100644
--- a/selfdrive/visiond/models/commonmodel.c
+++ b/selfdrive/visiond/models/commonmodel.c
@@ -97,39 +97,70 @@ static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, con
for (int i=0; ioutput[i]); } printf("\n");
@@ -71,7 +75,8 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
//fclose(f);
//sleep(1);
- //printf("done \n");
+ //printf("%i \n",OUTPUT_SIZE);
+ //printf("%i \n",MDN_GROUP_SIZE);
s->m->execute(net_input_buf);
// net outputs
@@ -79,6 +84,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
+ net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
ModelData model = {0};
@@ -91,9 +97,9 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
}
- model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
- model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
- model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
+ model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
+ model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
+ model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]);
model.path.prob = 1.;
model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
@@ -105,17 +111,59 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
+ // current lead
int mdn_max_idx = 0;
for (int i=1; i net_outputs.lead[mdn_max_idx*5 + 4]) {
+ if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
mdn_max_idx = i;
}
}
- model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*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 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;
}
diff --git a/selfdrive/visiond/models/monitoring.cc b/selfdrive/visiond/models/monitoring.cc
index 7b151d0c4..9e874aa0e 100644
--- a/selfdrive/visiond/models/monitoring.cc
+++ b/selfdrive/visiond/models/monitoring.cc
@@ -37,9 +37,11 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
s->m->execute(net_input_buf);
MonitoringResult ret = {0};
- memcpy(ret.vs, s->output, sizeof(ret.vs));
- ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1];
-
+ memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
+ memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position);
+ memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
+ memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
+ memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
return ret;
}
diff --git a/selfdrive/visiond/models/monitoring.h b/selfdrive/visiond/models/monitoring.h
index f9f4516bd..80c65d0f0 100644
--- a/selfdrive/visiond/models/monitoring.h
+++ b/selfdrive/visiond/models/monitoring.h
@@ -8,11 +8,18 @@
extern "C" {
#endif
-#define OUTPUT_SIZE 8
+#define OUTPUT_SIZE_DEPRECATED 8
+#define OUTPUT_SIZE 31
typedef struct MonitoringResult {
- float vs[OUTPUT_SIZE - 1];
- float std;
+ float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1];
+ float std_DEPRECATED;
+
+ float face_orientation[3];
+ float face_position[2];
+ float face_prob;
+ float left_eye_prob;
+ float right_eye_prob;
} MonitoringResult;
typedef struct MonitoringState {
diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc
index 530fd68f9..dd19912b3 100644
--- a/selfdrive/visiond/visiond.cc
+++ b/selfdrive/visiond/visiond.cc
@@ -716,6 +716,8 @@ void* monitoring_thread(void *arg) {
MonitoringResult res = monitoring_eval_frame(&s->monitoring, q,
s->yuv_front_cl[buf_idx], s->yuv_front_width, s->yuv_front_height);
+ double t2 = millis_since_boot();
+
// send driver monitoring packet
{
capnp::MallocMessageBuilder msg;
@@ -725,17 +727,28 @@ void* monitoring_thread(void *arg) {
auto framed = event.initDriverMonitoring();
framed.setFrameId(frame_data.frame_id);
- kj::ArrayPtr descriptor_vs(&res.vs[0], ARRAYSIZE(res.vs));
- framed.setDescriptor(descriptor_vs);
+ // junk 0s from legacy model
+ //kj::ArrayPtr descriptor_DEPRECATED(&res.descriptor_DEPRECATED[0], ARRAYSIZE(res.descriptor_DEPRECATED));
+ //framed.setDescriptor(descriptor_DEPRECATED);
+ //framed.setStd(res.std_DEPRECATED);
+ // why not use this junk space for reporting inference time instead
+ // framed.setStdDEPRECATED(static_cast(t2-t1));
+
+ kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
+ kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
+ framed.setFaceOrientation(face_orientation);
+ framed.setFacePosition(face_position);
+ framed.setFaceProb(res.face_prob);
+ framed.setLeftEyeProb(res.left_eye_prob);
+ framed.setRightEyeProb(res.right_eye_prob);
- framed.setStd(res.std);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s->monitoring_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
- double t2 = millis_since_boot();
+ t2 = millis_since_boot();
//LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;